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ABSTRACT 


A general method for establishing a kinematic model of a robotic manipulator with 
either a full or partial pose calibration system is developed. The theory applicable to 
modeling of mechanisms is introduced, as is rovotic manipulator calibration. Given a 
general over-specified kinematic model, a method is developed, with the associated 
algorithms for a six degree of freedom manipulator, that identifies non-unique parameter 
sets for any given partial pose measurement system. This method is applied and 


demonstrated on three existing calibration methods. 
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I. INTRODUCTION 


The study of robotic manipulator calibration is a continuing quest to improve the 
accuracy of the manipulator, thereby increasing its effectiveness and usefulness in the 
workplace. Manipulator accuracy is the precision to which the robotic system can place 
the end effector, or tool, in a commanded position and orientation, defined as the pose, 
within the working volume [Ref. 1]. A commanded pose is not one that has been 
previously taught to the manipulator, but one that is directed from an outside source, such 
as a computer data base. In order to achieve a commanded pose, the robotic system must 
calculate the joint variables required for the given pose location and orientation within the 
work space. This is accomplished by solving the inverse solution to the kinematic model 
of the manipulator. Accuracy of general industrial robots can be as poor as 10mm 
between the commanded and actual poses. [Ref. 2]. 

Repeatability is defined as the precision to which a manipulator can reacquire a 
previously taught pose. This differs from accuracy in that the manipulator is placed in the 
desired pose within the working volume, and the associated joint variables are then 
recorded for later use during the work cycle. In contrast to the aforementioned accuracy 
statistic, current manipulators have a high degree of repeatability, on the order of 0.1mm. 
{Ref. 1] 

As industry demands increase for both computerized process management and 
computerized sietion and reconfiguration of both repetitive tasks and unique, single 


occurrence tasks, robotic manipulators are required to be programmed off-line and to 
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share control programs. Off-line programming and shared control programs require a 
much higher degree of accuracy than currently exists -- a degree of accuracy more in line 
with current repeatability statistics. 

Factors that impact the accuracy of a robotic manipulator include: (1) inaccurate 
knowledge of the robot geometry and kinematic parameters, (2) link and joint compliance, 
(3) steady state servo controller errors, (4) gear backlash, and (5) temperature variations 
[Ref. 3]. Since it would be extremely expensive to manufacture a robot with the required 
stiffness and tolerances to negate the above issues and achieve the desired accuracy, the 
resulting errors must be identified and accounted for. The calibration process is the 
identification and sonrection of these errors. For the purposes of this work, calibration 
will only deal with the accurate identification of a manipulators actual geometry, or 
kinematic parameters. 

There are four basic steps to the calibration process: [Ref. 4,5] 

1. Aclosed chain kinematic model of the manipulator and measurement system is 
developed. This involves the determination of the identifiable kinematic parameters that 
form the complete model. This complete model is used to determine an error quantity 
based on the nominal kinematic parameter set and the actual measured set. The nominal 
parameter values are provided by the manipulator manufacturer and the measurement 
system specifications and location. 

2. Experimental measurements of the robot pose (partial or complete) are: taken. 


These measurements are a function of the actual parameter set. 


3. The actual kinematic parameter values are identified by systematically adjusting 
the nominal parameters until the model predictions match the measured values, driving the 
error functions, defined in the modeling phase to zero. 

4. The final step is to incorporate the identified parameter set into the controlling 
software for the manipulator. 

This work addresses the first step of the calibration process, the development of the 
closed chain kinematic model. Standardized and reliable approaches exist for modeling 
any given manipulator itself. Yet, in order to perform a calibration, the model employed 
must also incorporate the kinematics of the measurement system (full or partial) that is 


used to get the experimental data. Determining this complete closed chain model is a 


process that is neither obvious nor straightforward. The most difficult issue in closed 
chain model development is whether or not an existing kinematic parameter can be 
identified. The result of this thesis is a standardized method for determining the complete 
closed-chain kinematic model for any manipulator and measurement system. This was 
accomplished by defining a general, over-specified model, then conducting a calibration 
simulation with this model in order to generate the system Jacobian. Finally, parameters 
were systematically removed until a full rank Jacobian resulted, thus defining the complete 


kinematic model. 


Ul. THEORY 


A. KINEMATIC MODELING 

In order to understand how a manipulator achieves a commanded pose, one must 
understand the format of the kinematic model. When a commanded pose is given to the 
manipulator, it is specified relative to a world coordinate frame in the workspace. This 
world coordinate frame is then moved through each link of the manipulator by a 
coordinate transformation operation until it reaches the tool. The result is a description of 
the commanded pose in joint space. This set of coordinate transformations makes up the 
kinematic model of the manipulator. This section deals with the development of the 
coordinate transformations and the kinematic model. The theory discussed and some of 
the diagrams used in this chapter closely follow discussions by Paul [Ref. 9] and Mooring, 
Roth and Driels [Ref. 10]. 

1. General Coordinate Transformations 

As just described, the fundamental basis for kinematic modeling of a manipulator is 
the’ successive movement of a set of coordinate axes from one position and orientation in 
space to another. There are two building blocks that comprise the general coordinate 
transformation, the rotation transformation and the translation transformation. For the 
rotation transformation, consider two coordinate frames, where frame 2 has been rotated 


an angle } about the z axis of frame 1 (Figure 1). Note that the frames remain coincident 


and share the same z axis, allowing a two dimensional representation of the x-y plane 


(Figure 2). As illustrated in Figure 2, a point p is described in frame 2 by the vector 


& oit., ath Po =Pui2 +Ppya) +pak (1) 
where i 2, j 2 and kz represent the x, y and z unit direction vectors of coordinate frame 2. 


Likewise, point p can be described in frame 1 by the vector 


Pi =pxli +Dyij 1 + paki (2) 


Z, 5) 229 


Figure 1. Frame rotation about the z axis. 


The position of point p in frame 1 can then be related to the position of point p in 


frame 2 by: 
Px =Pr2C0S - pysing (3) 
Pyi = Px2Sind + py2Ccosd (4) 
Pu * Po (5) 


Transferring these equations into matrix form results in equation 6: 


Px cos@ —sing 0 || Pe 
sind cosh 0 | py (6) 
Pz 0 0 l pa 
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ao) 
= 
it 


Br =rot(z,o)p2 (7) 


The notation rot(z, ¢) represents a rotation about the z axis of angle ¢. Similar analyses 
can be conducted for rotations about the x and y axes. The results of these analyses will 


be presented later. 


Figure 2. 2-D view of frame rotated @ the z axis 


The translation transformation can be thought of as moving the origin of a 
coordinate frame to a new point described by the vector ¥, as illustrated in Figure 3, 


where 


X 
val y (8) 
Z 


form 


Figure 3. Translation of a coordinate frame 
B1 = rot(z,p)p2 + V (9) For 


convenience, equation 9 is expressed as a single matrix equation: 


Dxt Ppx2 
Py te T Py2 (10) 
Pz Pz 

WwW Ww 


Where T is a 4x4 augmented matrix of the rotation and translation transforms and 


w is a non-zero scale factor. For the example represented in equation 9, T is of the form: 


cosd -sing 0 x 


T= sind cosd 0 y 1 
0 0 12 oy 
0 0 Ow 


The matrix T is defined as the transformation matrix, and in the above example is a 
product of the rotation about the z axis and translation transformation matrices. Equations 


12 through 15 list the general form of the translation matrix and all the rotation 


pC 


transformation matrices. Unless dealing with a perspective transformation, the scale factor 


w equals one. 


(12) 


0 
0 cosy -siny 0 
0 snw cosy 0 
0 0 0 w 


Rot(x,y) = (13) 


cos6 0 sin€ 0 
0 1 0 0 
—sin® 0 cos 0 
0 0 0 w 


Rot(y,8) = (14) 


cos -sind 0 0 
sind cosd 0 0 
0 0 10 
0 0 Ow 


Rot(z,o) = (15) 


As evidenced in the previous example, a general coordinate transformation can be 
the product of individual rotation and translation transformations. The transformation 
represented in equation 16 can be thought of as a series of individual transformations, that 
when read left to right, represent a transformation operation on, or relative to, the 


resulting coordinate frame of the previous transformations. 


100x 0010 0-100 
0 1 

Trans(x,y,z)Rot(y,90)Rot(z,90) = 00 , : ie ; ; : A ; : ; (6) 
000w 000wi;i 00 0w 


Therefore, equation 16 represents a transformation that is a translation of the base 
frame origin in the x,y,z directions respectively, then a 90 degree rotation about that new 
frame's y axis, then a 90 degree rotation about the z axis of the most recent coordinate 
frame. 

2. Euler Transformations 

A six parameter transformation is required to transform from one coordinate frame 
that is fixed in space to another frame, also fixed in space. That is, three rotations are 
required to align the respective axes, and three translations are required to bring the 
origins coincident. As may have been realized in the previous section, the order that the 
transformations occur is important. This begs for the use of a standard frame to frame 
transformation to avoid confusion. 

The standard used in this work is the Euler transformation (Equation. 17), also 
known as the Roll, Pitch, Yaw transformation and will be represented as 
RPYT(O,9,v,x,y,z) (Ref. 11]. 

RPYT(6,9,w,x,y,z) = Rot(z,o)Rot(y,8)Rot(x, y)Trans(x,y,z)(17) This 
can be read as a rotation ¢ about the z axis, followed by a rotation @ about the new y axis, 
followed by a rotation w about the new x axis, then an x, y, z translation in the direction 
of the new x, y, z axes. 
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3. Denavit-Hartenburg Transformations 

A manipulator is made up of a sequence of links that are connected to each other 
by joints. A coordinate frame is associated with each link and a transformation matrix is 
required to describe the relative positions of successive frames. Since the links fix the 
geometric relationship between joints, there can be a reduction in the number of rotations 
and translations required by the Euler transformation. The Denavit-Hartenburg 
transformation provides an accepted systematic approach used to define link geometry and 
coordinate frame placement, resulting in reduced parameter transformations between link 
coordinate frames. For an n degree of freedom manipulator, there will be n links and n 
joints. Figure 4 illustrates a PUMA 560 six degree of freedom manipulator. Link and 
joint labeling starts at the base of the manipulator and progresses through to the tool, with 
joint n preceding link n. The base of the manipulator is defined as link 0, and not 
considered to be one of the n links. Therefore link 1 is joined to link 0 by joint 1 and link 
2 is joined to link 1 by joint 2, and so on. 

A generic link geometry can be characterized by two dimensions, the link length 
and link twist angle, illustrated in Figure 5. The link length, a,, is the length of the 
common normal between the joint axes n and n+J. The link twist angle, a@,, lies ina 
plane that is perpendicular to the common normal and located at the intersection of the 
common normal and joint axis n+J. This plane, therefore, contains joint axis n+] anda 
line that is a parallel projection of joint axis n. The twist angle, a, is the angle formed by 


the intersection of these two lines in the plane. 


1] 


The relationship of one link to the next, connected by a joint, is described by the 
distanced, d,, and angle, 6,, between links, as illustrated in Figure 6. Each joint axis n will 
have two comman normal lines intersecting it, one for link n-J and one for link n. The 
distance between the intersection points of the two common normal lines on the joint axis 
is the distance between links, d,. The angle between links, @,, lies in a plane perpendicular 
to joint axis 7 at the intersection of joint axis m and the common normal for link n-J (N,). 
Similar to the definition for @,, this plane contains the line N, and a parallel projection of 


the common normal for link n (N, and N,'). The angle between links is then the 


counterclockwise angle between N, and N,.. 


Figure 4. PUMA 560 robot manipulator with links and joints labeled 
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— Joint n+l 


Figure 5. Generic manipulator link 

With the link and joint geometries defined, coordinate frames can be assigned to 
each link. For a revolute joint, illustrated in Figure 6, the angle between links, 6,, is the 
joint variable. The origin of the coordinate frame for link 7 is located at the intersection of 
joint axis n+J and the common normal for link n (N, in Figure 6). The x, axis is aligned 
along the common normal (N,), and the z, axis is placed along joint axis n+J. The joint 
variable 9, is in the zero position when x, and x,., are parallel. There are two exceptions 
to this method of placement, specifically when two successive joint axes either intersect or 
are parallel. For intersecting axes (n and n+), the link length a, is set to zero. The 
frame origin is placed at the intersection point of the joint axes. The x, axis direction is 
normal to the plane that contains both joint axes n and n+J. For parallel axes there is not 
a unique common normal, and the distance between joints, d,, can be arbitrarily chosen. 


Therefore the frame origin is placed on the n+J joint axis at a point that will make the 
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distance between joints zero for the next link, d,,,, whose coordinate frame origin is 


defined. 


Joinina ae Joint nel 


| hb O25 


Joint a-i 


ei Link a-l 


Link a+! 


Figure 6. Revolute joint with link parameters 


For a prismatic joint, illustrated in Figure 7, the joint variable is the joint distance 
d,. Unlike a revolute joint, where the circular path of a point rotating about an axis 
defines both the plane in which the rotation occurs and the location in space of the axis 
about which the point rotates, the path of a point moving along a straight line , as ina 
prismatic joint, only defines an axis direction and not the location of the axis in space. 
Therefore, the link length, a,, is meaningless for a prismatic joint. As a result, the 


coordinate frame for a prismatic joint is placed coincident with the next defined link 
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coordinate frame origin. The z, axis direction lies along the n+/ joint axis with the x, 


direction perpendicular to both the n+J joint axis and the prismatic joint direction. 


Jointn 


ttt H Joint n+! 


Joint n-i 


Link n-J 


Link n+} 


Figure 7. Prismatic joint with link parameters 


Having defined the geometric relationships between links and the joint coordinate 
frame, the transformation matrices can be developed. The Denavit-Hartenburg 
transformation uses the translation and rotation matrices discussed previously to step from 
frame to frame in a logical manner. Referring to Figure 6, the transformation can be 
completed in four steps: 

1. Rotate an angle @, about the z,_, axis. 

2. Translate a distance d, along the z,,, axis. 


3. Translate a distance a, along the new x axis (same direction as X,). 


1S 


4. Rotate an angle a, about the x, axis, aligning the z axis with the n +J joint axis. 

The resulting transformation matrix is described by equation 18: 

A, = Rot(z,0,)Trans(z,d,)Trans(x,a,)Rot(x,c,) (18) 

4. Modified Denavit-Hartenburg Transformation 

The standard Denavit-Hartenburg transform provides an accurate model of a 
manipulator for most forward kinematic solutions. But in the case of manipulator 
calibration, where the kinematic parameters become the variables, potential problems arise 
for parallel or nearly parallel consecutive axes. 

Consider two parallel axes where a common normal does not exist between the 
joint axes, and the coordinate frame placement is a matter of convenience. If in fact the 
two axes are not actually parallel, then a common normal does exist between the two, 
therefore fixing the position of the coordinate frame. This could result in a 
disproportionately large change in the kinematic parameters that then cascades through the 
remaining links and joints. These disproportional changes cause numerical instability in 
the parameter identification process. 

The Modified Denavit-Hartenburg transformation solves this problem by removing 
the requirement for a common normal between joint axes. As illustrated in Figure 8, this 
is accomplished by defining a plane that is perpendicular to joint axis n and intersects joint 
axis n at the origin of the n-] coordinate frame. The intersection of joint axis n with this 
plane defines the origin of coordinate frame n, and a line in the plane from frame n-J to 


frame n. [Ref. 12] 
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| Joint a-l 
AXIS 


Figure 8. Modified Denavit-Hartenburg transformation, after Ref. [12] 
The transformation equation is then 
A, = Rot(z,8,)Trans(x,r,)Rot(x,c.,)Rot(y,B,) (19) 

which consists of: 

1. A rotation of angle 6, about the z,_, axis 

2. A translation of distance 7, along the line connecting the coordinate frame 

origins n-] and n, which is also the new x axis direction. 

3. Arotation of a, about the new x axis 

4. Arotation of £, about the new y axis to align the z, axis direction with joint 

axis n+. 

Equation 20 can then be used for any type of manipulator coordinate frame 


transformation where , is set to zero for standard Denavit-Hartenburg transformations 
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applied to joints with non-parallel consecutive axes, and d, is set to zero for the 
transformations between joints with nominally parallel axes. 
A, = Rot(z,0,)Trans(z,d,)Trans(x,a, )Rot(x,c,)Rot(y,B,) (20) 
Equation 20 is the Modified Denavit-Hartenburg transformation equation that is 
used in all calibration procedures in this work. 
5. The Kinematic Chain 
Once homogeneous transformation matrices(whether Euler or Modified 
Denavit-Hartenburg) have been developed for a single coordinate frame transformation, it 
is time to describe the pose of the end effector of a manipulator with respect to the world 
coordinate frame of the work space. As mentioned previously, this is accomplished by 
stepping from frame to frame, beginning at the world coordinate frame and ending at the 
end effector coordinate frame. This path of homogeneous transformations is commonly 
referred to as the kinematic chain. The kinematic chain is formed by sequentially 
post-multiplying the transformation matrix chain by the next link homogeneous transform, 
as in equation 21. 
Tye = Ay’Ag)-AysA? (21) 
The 'A' matrices are either Euler transforms, as for A,,° and A,", or Modified 
Denavit-Hartenburg for A,'...A,,". The notation used in equation (21) represents a 
transformation from the world coordinate frame, W (subscript), to the end effector, E 


(superscript). The kinematic chain is illustrated graphically in Figure 9. 
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Figure 9. Kinematic chain 


6. General 30 Parameter PUMA Manipulator Model 

Armed with the appropriate tools to model a manipulator, a model can be 
generated for the PUMA 560 six degree of freedom manipulator illustrated in Figure 4. 
The PUMA is used as the reference manipulator throughout this work. Figure 10 shows 
the PUMA with the Modified Denavit-Hartenburg coordinate frames attached, as well as 
the world coordinate frame, F,,, the manufacturers designated base frame, F,, and an end 
effector frame, F,. The transformations from frame (1) to frame (5) each contain the 
standard four parameters from the Modified Denavit-Hartenburg transformation, but the 
transforms from the world frame to frame (1) via the base frame need to be considered 


carefully since potential parameter dependencies exist. 
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Figure 10. PUMA 560 coordinate frame allocation 


In order to ensure the minimum number of parameters required to move from the 
world coordinate frame to frame (1), two possible paths may be taken. [Ref 13] The 
first path involves three Denavit-Hartenburg transformations, shown in Figure 11: 

1. A four parameter transformation from the world coordinate frame to an 

intermediate frame, designated frame (0). 

A, = Rot(z,0,)Trans(z,d,)Trans(x,a,)Rot(x, 9) (22) 
2. A Denavit-Hartenburg transform from frame (0) to frame (b) that only requires 
two of the four Denavit-Hartenburg parameters, 6,, d,. 
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A, = Rot(z,8,)Trans(z,d,) (23) 
3. A four parameter Denavit-Hartenburg transformation from frame (b) to frame 
(1) 


A, = Rot(z,8,)Trans(z,d,)Trans(x,a,)Rot(x,a,) (24) 


Figure 11. Base transformations 


The result of this path is an eight parameter transform from the world coordinate 
frame to frame (1), since 9, and ¢, are both about the z, axis and cannot be identified 
independently, and d, and d, are both along the z, axis and cannot be distinguished 
independently either. 

‘The second path involves only two transforms to get from the world coordinate 
frame to frame (1). 


1. A full six parameter Euler transform from the world frame to the base frame. 
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A, = Rot(z,o,)Rot(y,6, )Rot(x, y,)Trans(x,,y,,z,) (25) 
2. A Denavit-Hartenburg transform from the base frame to frame 1. 
A, = Rot(z,0,)Trans(z,d,)Trans(x,a,)Rot(x,a,) (26) 
As with the first path, a total of eight parameters are required to get from the 
world frame to frame (1), since q, can be resolved into f,, g,, y, and d, can be resolved 
into X,, y,. Z, Note that both paths require eight parameters, but not the same set of 
eight. For this work the second path is used. 
The last transform in the kinematic chain is from frame (5) to the tool, frame (6). 
This is a full frame to frame transformation and requires a six parameter Euler 
transformation. 
= Rot(z,b,)Rot(y,0,)Rot(x, y,)Trans(x,,¥¢,Z,) (27) 
Table 1 provides the nominal values of the 30 parameters required for the PUMA 


560, where the parmeters in bold type are not identifiable. 


i 
H 
fl 


Table 1. Nominal kinematic parameters for PUMA 560 
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7. 28 Parameter SRIP Manipulator 

Another manipulator model used in this work is the Driels and Pathre 5R1P 
manipulator shown in Figure 12. The SRIP is a six degree of freedom manipulator, 
similar to the PUMA 560, but with 5 revolute joints and one prismatic joint. As discussed 
in Section 3, the length a, for the prismatic joint 3 has no meaning and is defined to be 
zero. With the location of the prismatic joint coordinate frame (frame 3) determined by 
the common normal between axes 4 and 5, and placed coincident to the origin of frame 
(4), the length d, is also, by definition, equal to zero. The kinematic model for the 5R1P is 


then a 28 parameter model with the nominal parameters listed in Table 2. 


Table 2. Nominal kinematic parameters for SR1P manipulator 


B. PARAMETER IDENTIFICATION METHODOLOGY 

1. Experimental Measurement 

Assuming an appropriate, complete kinematic model of the manipulator has been 
developed with nominal kinematic parameter values, actual parameter value identification 
or calibration can be performed. Experimental measurements of the end effector pose are 


taken. These can be either full or partial pose measurements, depending upon the 
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measurement system used for calibration. Partial pose measurements do not measure all 
six pose elements (three rotations and three translations). Complete kinematic models for 
partial pose measurement systems have fewer than the full 30 parameter PUMA model, 
depending on the constraints applied by the measurement system. For each pose, the 
measured pose data and the associated joint angles are recorded. This process is repeated 
for additional poses until an adequate data base is collected. An adequate data base is 
dependent on the number of kinematic parameters to be identified (unknowns), and the 
number of known pieces of data measured on each pose. For a full pose measurement 
system, a minimum of N/6 different pose measurements must be taken, where N is the 
number of parameters to be identified. This number must then be increased to account for 
measurement noise and the degree of accuracy desired. 

2. Identification 

Once experimental measurements have been taken, with joint angles recorded for 
each pose, the kinematic parameter values can be identified. One approach to identifying 
the kinematic parameters is to determine the differential relationship between the pose 
variables, P, and the kinematic parameters, K [Refs. 2,15]. The differential pose variables 
are related to the differential kinematic parameters, or parameter error vector, by the 
Identification Jacobian. 

SP = J&K (22) 
The differential pose variable vector, 5P, is a p x 7 column vector, where p is equal 


to 6 for a full pose measurement system: 
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Axis 2 


Figure 12. SR1P Robot manipulator & coordinate frames, from Ref. [14]. 
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gp =| %: (22) 


dp, 
dp z | 


The Jacobian is a p x n matrix, where n is the number of parameters in the complete 


kinematic model. The parameter error vector, 5K, is an n x I column vector of the 
differential parameters. For multiple pose measurements each differential relationship is 
appended on to the previous one, resulting in the equation: 
dT = J&K (23) 
Here OT is a (p x m) x 1 vector, J is a (p x m) x n matrix, 5K is still an n x 1 vector, and m 
is the number of pose measurements taken. Equation 23 can then be inverted using the 
Jacobian pseudo-inverse (since the Jacobian is not a square matrix), to solve for the 
kinematic parameters directly: 
8K = (JJ]''ST (24) 
To avoid solving equation 23 directly, a multidimensional optimization routine is 
used to minimize a user defined error function based on the difference between the pose 
data calculated from the nominal kinematic parameters and the actual pose data. 
3. Optimization 
An accurate approximation of actual kinematic parameters can be achieved by 
systematically changing the nominal parameter set in a non-linear, least squares sense. 
This is done until a minimum value is reached for the error function relating the actual and 


nominal kinematic parameters [Ref. 16]. This non-linear least squares minimization of a 
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pose data error function has been employed with success on numerous full and partial 
pose robot calibration systems [Refs. 6,7,8, 13]. 

The IMSL library routine ZXSSQ is a FORTRAN based algorithm that will 
minimize a function over a given variable set using a Levenburg-Marquardt non-linear 
least squares method. At each iteration in ZXSSQ, the next estimate of the variable set is 
based on a numerical approximation of the Jacobian. The Jacobian approximation is 
arrived at using forward or central difference methods around the current parameter set. 
Upon satisfying the designated convergence exit criteria, ZXSSQ returns the most recent 
estimate of the parameter set. Figure 13 illustrates the program flow of ZXSSQ where N 
is the size of the variable set vector, x. ZXSSQ uses an external user-defined subroutine 
that contains the error function to be minimized. Using this subroutine, it calculates the 
finite difference gradient. Based on that gradient, it then re-evaluates the function and 
tests for convergence. 

The error function supplied to ZXSSQ is generated by calculating the differential 


transformation matrix, A, as described by Paul [Ref. 7]: 


0 -6; dy d. 

& 0 -d. d 
ee ie aks 25 
28y Oy (0 a; 2) 

0 0 0 0 


The A matrix is generated by calculating the forward kinematic solution of the 
manipulator, T®*, using the nominal kinematic parameters, T®, for each set of joint angles. 
The forward kinematic solution is again calculated, but using the experimental pose 
measurement data, T®™. The difference between T® and T®” is the difference matrix AT. 
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The elements of the AT matrix, t,, are used to calculate the elements of the differential 
transformation matrix, A, equations 26-31, where the indices i and j represent the 
respective row and column of the matrix. The A matrix is only valid when the tT a0d 


matrices are almost equal. 


f32~t3 | (26) 
| (27) 


USER SUPPLIED SUBROUTINE 


CALCULATE : CALCULATE 
MINIMIZING | GRADIENT 
FUNCTIONS : 


Figure 13. Program flow for ZXSSQ 
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b= [5 as 


d= ti, (29) 
dt (30) 
dts (31) 


The error function is the sum of the squares of the elements of the A matrices for the 


entire set of measured poses, equations 32 and 33. 


ES (fy(0)" (32) 

i=l yal 

f, (x) = 5, 

f(x) = 5, 

f 3(x) =6, 

£40) = 4, ay = 
£ s(x) = d, 

f(x) =d, 


Note that for a partial pose measurement the error function is a sum of the squares of only 
those elements in the A matrix that correspond to the actual measurement data taken. In 
other words, if only position data is measured, and no orientation data, then the error 
function will only use d,, d,, and d, for all the pose measurements. 

In order to ensure that possition and orientation are equally weighted and 


represented when minimizing the error function, all elements in A need to be the same 


order of magnitude. Since the orientation error elements, 6,, 6,, and 6,, are not the same 


order of magnitude as the possition elements, they are multiplied by a scale factor to 
match and ensure a proportional optimization. 

4. General Identification Algorithms 

As stated in the previous section, ZXSSQ uses an externally defined error function. 
The program ID6 contains the error function calculation in the subroutine PUMA_ARM. 
ID6 program flow is illustrated in Figure 14. [D6 reads an input file with nominal 
kinematic parameter values for the particular model being calibrated. These values are 
then used to initialize the variable vector x in ZXSSQ. ID6 also reads the measurement 
data with the associated joint variable data, from another file, and passes all this 
information in the proper format to ZXSSQ. The final parameter values from ZXSSQ are 
output to a file with residual error values of position and orientation. 

A simulation of the calibration process is conducted to ensure the identification 
routine is correct, to predict the number of experimental pose measurements required to 
complete the identification, and to estimate the resulting accuracy of the manipulator after 
calibration. The sequence of simulation algorithms used is illustrated in Figure 15. 
Program JOINT is used to generate random joint data within the constraints defined by 
the work space and the pose measurement system. The program then writes this data to 
PUMA_VAR. Program POSE calculates the forward kinematic pose matrix solution for 
the associated joint data sets from PUMA_VAR, and writes the combined data to 
PUMA_POS. ID6 has been described previously. VERIFY confirms the accuracy of the 


identification routine by calculating a total position and orientation error from a 
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comparison of the forward kinematic solution, using the nominal parameter values and the 


identified values. 
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Figure 14. Program flow for ID6, form Ref. [8]. 
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Figure 15. Simulation process flowchart, from Ref. [7]. 
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Il. COMPLETE MODEL DETERMINATION 


A. INTRODUCTION 

As previously stated, determining the complete closed-chain kinematic model of a 
robot manipulator and calibration measurement system is the most difficult portion of the 
calibration process. A complete model, as defined for this work, is the kinematic model of 
the system with the minimum number of parameters required to achieve convergence to 
the actual parameter values during identification. If there are not enough parameters in 
the model, it does not fully describe the manipulator. Yet, with too many parameters the 
model is over-specified, and the identification routine will not converge to an accurate 
solution. Therefore, all the parameters in the model must be identifiable for it to be 
complete. This also implies that an over-specified model contains parameters that cannot 
be identified independently. Arriving at the number and type of identifiable parameters in 
a kinematic model is where a majority of the work and thought reside in model 
development. 

Although well established, closed-chain models exist for full pose measurement 
systems, less capable, partial pose measurement systems result in a reduction in the 
number of identifiable parameters from the full pose model. Determining the constraints 
and dependencies that exist in any given system is a difficult task since no clear, 
standardized approach exists to determine the set of parameters required to define a 
complete model. Therefore, model development frequently becomes an iterative, trial and 


error process. 
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B. THE CLOSED-CHAIN MODEL 

As stated previously, the closed-chain model incorporates both the manipulator 
and measurement system kinematics, Figure 16. Here the manipulator kinematics have 
been well defined using Denavit-Hartenburg or Modified Denavit-Hartenburg methods. 
The measurement system kinematics uniquely define the position and orientation of the 
end effector relative to the world coordinate frame. From this point on, the world 
coordinate frame may be referred to as the measurement system frame (F,,), since the 


world frame is usually placed at some convenient location in the measurement system. 


MANIPULATOR 
KINEMATICS 


MEASUREMENT 
SYSTEM 
KINEMATICS 


Figure 16. The closed chain model, after Ref. [8]. 


An in depth analysis of the transformation from the independently located world or 


measurement system coordinate frame to the manipulator base frame (T,, in Figure 16) 
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was completed in Chapter IJ. Recall that dependencies existed between the world-to-base 
transformation parameters and the manipulator kinematic parameters. Two different paths 
were taken, resulting in different parameter sets but the same number of parameters. 

The end effector transformation, T,,, is a six parameter transformation from the last 
link of the manipulator (n-/ for an n degree of freedom robot) to the independent location 
of the tool frame. The Euler transformations between the manipulator and measurement 


system are illustrated in Figure 17. 


INTERNAL 
MANIPULATOR 5 
KINEMATICS J 
RPYT($,6,¢.X,Y.Z) RPYT (6,8, ¥.%,Y12) 


| 


Figure 17. Manipulator and measurement system transformations, from Ref. [8]. 


Parameter dependencies may exist between the end effector transform and the 
manipulator kinematics. Careful end effector coordinate definition and placement, 


however, can usually avoid these dependencies. Additional parameter dependencies in 
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frames T,, and T, may exist due to constraints incurred by the type of partial pose 
measurement system used. 

Work by. Mooring, Roth, and Driels has determined limits for the number of 
parameters required to define a complete model [Ref. 18]. Using Denavit-Hartenburg 
definitions for manipulator kinematics, and including the requirement that any manipulator 
must be referenced to an external world coordinate frame, four parameters are required 
for each revolute joint (R) and two for each prismatic joint (P). With the additional six 
parameters required to ensure independent location of the end effector frame, equation 
(34) defines the required number of independent parameters, N, for completeness of any 
manipulator. 

N=2P+4R+6 (34) 
For the PUMA and 5RIP robots, N is equal to 30 and 28, respectively. Since any given 
partial pose measurement system will impose constraints that result in unidentifiable 
parameters, the value N becomes a maximum value. Therefore, for any six degree of 
freedom robot, there can be in no case more than 30 parameters in the complete, 
closed-chain model. Knowing that the measurement system can result in dependencies in 
the measurement and end effector transformations, T,, and T,, the total number of 
identifiable parameters associated only with the manipulator is given by equation (35) 


[Ref. 19]. 
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K=2P+4R-6 (35) 
For the PUMA, the number of identifiable parameters in the manipulator kinematic model 
is 18, 22 for the SRIP. 

The value K defines the number of parameters required for the manipulator to 
move from the manipulator's base frame to the last link, frame n-1. The internal base 
frame and frame n-] are unique for any given manipulator and set of joint angles. 
Therefore the shortest way to close the chain is by an Euler frame-to-frame transformation 
between the two frames. Given that the manipulator kinematics are unique, there can be 
no additional dependencies. Therefore, all six Euler parameters are independent. This 
sets the lower bound on the number of independent, and thus identifiable, parameters for a 
closed-chain model, equation (36): 

M=2P+4R (36) 
Thus the number of identifiable parameters in a closed-chain model, n, with any 


measurement system, is governed by equation (37): 
M<n2N (37) 


Once limits to the number of parameters required for a complete model are known, 
the field of view required to determine the complete model is narrowed. However, 
equation (37) provides no information regarding the specific number or type of parameters 
within those limits. It is understood that additional parameter dependencies and, 
therefore, identifiability, are affected by the type of motion constraint applied to the end 
effector, and the ei of the measurement data (length, azimuth, and elevation etc.) 


recorded by the system. Determining the relationship between the end effector motion 
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constraints, measured data, and parameter dependencies is the challenge in closed-chain 
manipulator modeling that often results in a trial and error process to determine the 
complete model. Attempts to generalize and classify these relationships in this work 
proved inconclusive. The following section investigates a method to circumvent these 
problems and arrive at a complete model for any closed-chain system regardless of 
relationships between the end effector motion constraints, measured data and the 
identifiable kinematic parameters. 
C. GENERAL METHOD 

The identification Jacobian for a complete kinematic model is a full rank matrix 
where the rank equals the number of identifiable parameters [Ref. 18]. Since a complete 
model contains only identifiable parameters, the rank behavior of the identification 
Jacobian is studied in detail. The rank of a matrix in a matrix equation, as in equation (22) 
repeated here, provides an indication of the number of independent equations within the 
system of equations represented in matrix form. 

5P = J6K (38) 

Therefore, it is assumed that if the rank of the identification Jacobian is less than the 
number of kinematic parameters (the number of elements in 5K), then the model is 
over-specified and dependencies exist. From Chapter II, Section B-2, the Jacobian isa p 
xX n matrix, where n is the number of columns and equal to the number of parameters in 
the model. Each column contains the coefficients for each respective differential kinematic 


parameter element in the vector 6K. Consequently, in the case of an over-specified model, 
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setting the column that multiplies a dependent parameter in the 6K vector equal to zero 
will not change the rank of the Jacobian. Likewise, removing a dependent parameter from 
5K and its associated column in the Jacobian will not change the rank of J. 

An over-specified, closed-chain model of a system is generated in this work, then 
the identification Jacobian rank behavior is investigated. If the matrix is not full rank then 
parameter dependencies exist. Those parameters (and their associated columns in the 
Jacobian), that, once removed, do not affect the rank of J, are then removed from the 
model. This is repeated until a full rank Jacobian is achieved. The resulting rank matrix 
will contain only those parameters that are identifiable and define a complete model. 

Since equation (34) defines the upper limit of the number of parameters in a 
complete model, the value N is used to define the over-specified model and parameter set. 
Once defined, the complete model determination process. In the case of the PUMA 
manipulator, this is the full 30 parameter model defined in Chapter II. 

D. GENERAL PARAMETER REDUCTION ALGORITHM 
1. Simulation 
One of the by-products of the optimization routine ZXSSQ used in the 
calibration process is a numerical approximation of the identification Jacobian. By running 
the first three steps of the calibration simulation (programs JOINT, POSE, and ID6), a 


Jacobian approximation can be extracted. 
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In order to determine the complete model parameter set for a six degree of 
freedom manipulator, the programs POSE and ID6 are written to use the over-specified 
30 parameter model for the PUMA manipulator defined in Chapter IT. 

Programs JOINT and ID6 are modified to represent the particular measurement 
system used in the calibration process. The end effector pose constraints imposed by the 
measurement system are accounted for when generating the random joint variable data in 
program JOINT. The error function in ID6 is modified for each system to reflect the error 
in only the parameters measured. 

Although program ID6 will not converge to an accurate solution of the actual 
kinematic parameters when using an over-specified model, the Jacobian can be extracted 
for use in the general parameter reduction program GPRED. Upon extraction of the 
Jacobian from the routine ZXSSQ, the Jacobian transpose product, J°J, is calculated and 
written to the data file JTJ.DAT. This forms an N x N square matrix that saves memory 
space and time, independant of the number of pose measurements simulated. 

2. Program GPRED 

The general parameter reduction program GPRED reads the J"J matrix from 
JTJ.DAT and calculates the rank. From the difference between the rank, R, and the J"J 
matrix dimension N (N is equal to 30 for the PUMA), the number of dependencies and, 


therefore, non-identifiable parameters is known, equation (39): 
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d=N-R (39) 
With GPRED using the J"J product, a row and a column need to be removed from the 
matrix to remove a parameter from the model. 

GPRED then systematically investigates the dependency of each parameter by 
removing the associated row and column from J"J and recalculating the rank. If the rank 
is reduced by one, then the parameter removed is independent and identifiable. GPRED 
then steps to the next parameter and tests it for dependency. If the rank does not change 
then the parameter is dependent and not identifiable. The process is then sequentially 
repeated with each successive reduced Jacobian until a full rank matrix is achieved and d 
non-identifiable parameters have been extracted. The parameters remaining in the full 
rank reduced J"J matrix define a complete model, kinematic parameter set. GPRED then 
steps back out of the reduction and investigates for other possible complete model 
parameter sets. 

The resultant non-identifiable parameter set vectors, associated with each 
complete model parameter set, are written to the file DDEP-_DAT. Note that, for multiple 
parameter sets, GPRED will find all possible permutations of each parameter set. As a 
point of reference, this process will be described as the parameter reduction process for 
the rest of this work. 

3. Complete Model Determination of Over-Specified 5R1P Manipulator 
In order to demonstrate the use of the complete model determination method 


described in the previous sections, the parameter reduction process was applied to the 
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SRIP manipulator with a full pose measurement system. Recall that the 5RIP is a 28 
parameter model with one prismatic joint. 

Starting with a 30 parameter over-specified model (the 30 parameter PUMA 
model from Chapter II), the parameter reduction should result in a 28 parameter set that 
would define a complete model. To do this, the identification simulation routines 
(programs JOINT and ID6) need to represent the prismatic joint. The 30 parameter 
PUMA models in the identification routines were modified by defining the translation 
parameter d, as the joint variable, vice 6, in the PUMA case, to accommodate the 
prismatic joint three. 

Running this 30 parameter over-specified model through the parameter reduction 
process revealed a number of sets of dependent, non-identifiable parameters. Of the five 
parameters listed in Tabie 3, any pair combination will define a set of non-identifiable 
parameters. These parameters could be removed from the 30 parameter over-specified 


model to form a 28 parameter complete model of the 5R1P manipulator. 
fixed oe ee I es 
Table 3. 5R1P non-identifiable parameters 
Table 3 shows that possible dependencies exist with the link length, a,, and 
distance between links, d,, for joints two and three and the distance between links for joint 
four. Previously, for a prismatic joint, n, the parameters a, and d,,, are known to be 


non-identifiable. For the SR1P manipulator, this corresponds to a, and d,. The general 
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parameter reduction process confirmed the dependency of parameters a, and d,, and also 
demonstrated that this set of parameters is not unique. Consequently, any given complete 
model parameter set for a manipulator is not unique. 

The parameter reduction process uncovered all possible permutations of the 
non-identifiable sets, indicating that the order of parameter removal, within the imbedded 


loops of the reduction process, has no affect on the outcome. 
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IV. VERIFICATION OF COMPLETE MODEL DETERMINATION 


As previously stated, standardized complete models of full pose measurement 
systems exist. Determining the complete model for a partial pose measurement system is @ 
different task. This chapter verifies the use of the general complete model on two partial 
pose, constrained measurement calibration systems, the modified linear slide [Ref. 8] and 
the fixed length ball bar [Ref. 6]. 

A. MODIFIED LINEAR SLIDE 

1. Physical Description 

The linear slide system was used to calibrate a PUMA 560 by Potter [Ref. 7] then 
modified by Swayze [Ref. 8] to allow for a greater range of joint motion during 
calibration. 

The linear slide, pictured in Figure 18, used the x axis of a coordinate measuring 
machine (CMM) to accurately measure the distance along the axis from some zero 
reference point. The original linear slide, used by Potter, bolted the tool flange of the 
PUMA to a flange on the CMM carriage. This fixed the orientation of the tool frame with 
respect to the measurement system axis. Swayze modified this system by adding an off-set 
ball and socket joint between the tool flange and the CMM carriage. This modification 
allowed for a greater range of motion over the original linear slide, thereby increasing the 


overall calibration accuracy. 
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Figure 18. PUMA with linear slide constrained measurement system. 


This system places a physical constraint on the end effector, only allowing it to 
move along the axis defined by the CMM. It measures the position of the end effector 
frame on that axis. 

2. Closed-chain kinematic model 

As noted previously, the 18 kinematic parameters of the PUMA manipulator, from 
the base frame to frame five, are unique and unaffected by different measurement systems. 


The manipulator kinematic parameters are listed in Table 1. Parameters of the 
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measurement system transformation, T,,” , and the end effector transformation, T,°, must 
be determined. 

For the end effector transformation, the ball joint provides three degrees of 
freedom. This allows rotation about the three axes of a frame placed at the center of the 
ball. The free rotation and fixed translation of the ball joint, therefore, define a point in 
space. The end effector frame is placed at this point, but the orientation of the frame is 
undefined. 

The end effector transformation, therefore, moves from frame (5) to a point in 
space. Any three of the six Euler parameters are necessary to make this transformation, 
but one parameter must be a translation. Otherwise displacement of the origin is not 
possible. 

As the end effector and carriage assembly move along the CMM track, they define 
an axis, not just a direction. The origin of this axis is located at the zero point for the 
CMM readout. This zero point is the origin of the measurement system coordinate frame. 

The measurement system transformation is developed by considering the base to 
world transformation, T,™, then inverting it. A three parameter transformation is required 
to locate the origin at a point in space. Two rotations are then required to align any one 
axis of the frame with the CMM track. This results in a five parameter transformation. 
Table 4 is the parameter list, developed by Swayze [Ref. 19], for the two transformations, 
pictured in Figure 19.. Note that the non-identifiable parameters listed in Table 4 are 


indicated with a bold zero (0). With these eight parameters added to the 18 for the 
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manipulator kinematics, there are 26 identifiable parameters in the complete closed-chain 


model. 
} Ty Le 
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Table 4. Identifiable kinematic parameters for the modified linear slide. 


Axis of Linear Slide 


Figure 19. Linear slide transformations. 


3. GPRED Results 
With the error function properly defined in the simulation programs, a 


30-parameter, over-specified model on the modified linear slide was run through the 
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parameter reduction process to determine the complete model. The error function, e, is 
the length measurement difference along the linear slide axis, represented by equation 
(40): 

e=[ jx? + y* +2273 (40) 
where the measured distance is x, with y and z nominally zero, and / is the readout from 
the CMM system display of the actual value of x. 

The rank of the Jacobian for the over-specified, modified linear slide was 26, 
confirming a 26 parameter complete model. GPRED then found 48 possible parameter 
sets (Table 5) that each result in a full ranking matrix, and, therefore, define a complete 
model. GPRED confirmed the parameter set listed in Table 4. Note Table 5 is a 
condensed representation of the total set of 48 parameter sets. The 16 end effector 
parameter sets, when added to the measurement system parameter sets with either ¢,, 6,, 
or y,, removed individually, make up the 48 sets. The x indicators represent identifiable 
parameters. 

In order to confirm that other parameter sets are indeed valid and define a 
possible complete model, one was chosen arbitrarily for simulation. The simulation code 
described in Chapter II, Section 2-B was modified to reflect the 26 parameter set partially 
listed in Table 6: 

The calibration simulation using the parameters in Table 6, with no noise, 
converged to within 1.0E” of the actual kinematic parameter values in the simulation. 


This result confirms that a given complete model for a manipulator is not unique, and 
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that any of the resulting parameter sets could be used to define a closed-chain, complete 
model. The types of parameters required for the end effector and measurement system 
transformations were consistent with those discussed in the previous section. 
Specifically, they were consistent with a five parameter transformation from the 
measurement system to the base frame, where one rotation parameter is not identifiable. 
For the end effector transformation, three parameters were identifiable, with one 


translation in every parameter set. 
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Table 5. Complete model parameter sets for modified linear slide 


GPRED again retumed all possible permutations of every parameter set, proving 


that the order of parameter removal within the process had no bearing on the end result. 
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B. FIXED LENGTH BALL BAR 

1. Physical Description 

The second constrained, partial pose measurement system considered is a ball bar 
of fixed length, developed by Driels [Ref. 6]. The ball bar constraint system used for 
manipulator calibration, illustrated in Figure 20, is a rigid bar of fixed, known length with 
a ball and socket joint at each end. One ball and socket joint is fixed to the robot's work 
bench, the other is attached to the end flange of the last link of the manipulator. The end 
effector is then physically constrained to a surface of fixed radial distance from the 
reference point in the world or measurement system coordinate. The end effector is thus 
limited to placement in a hemi-spherical surface, with a radius equal to the length of the 


bar. 


Figure 20. PUMA with ball bar constrained measurement system. 
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Table 6. Verified parameter set for the modified linear slide. 


2. Closed-Chain Model 

As with the modified linear slide, the 18 parameters associated with the PUMA 
manipulator are unchanged. The measurement system and end effector frame placement, 
and transformations, follow a similar development to the ball joint end effector of the 
modified linear slide. 

The location of the measurement system coordinate frame is chosen to be at the 
center of the ball at the lower end of the bar. As with the modified linear slide end 
effector, the ball joint defines a point in space with no frame orientation information. 
Again, the transform from the base frame to the measurement system is considered, then 
inverted. This is a frame to a point transformation, thus requiring three parameters, one of 
which must be a translation. The measurement system frame is chosen to have the same 
orientation as the base frame, therefore, three translation parameters, x,,y,, and z,, 
comprise the identifiable parameters in the measurement system transformation. 

The end effector transformation is also a frame to a point. The ball joint at the end 
effector is displaced from the manipulator's frame five axis, z,, in order to allow 


identification of the encoder off-set, or joint variable, for joint six, 56, (refer to Figure 21). 


ay. 
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The three parameters ¢,, x,, and Z,, are chosen for the frame-to-point transformation. The 
parameter ¢, was chosen as it inherently includes both the joint variable for joint six, and 
the constant off-set to account for the mounting plate orientation on the PUMA's end 
flange. The measurement system and end effector frame transformation parameters are 


listed in Table 7. 


Table 7. Ball bar complete closed-chain model parameters. 


Figure 21. Ball bar end effector transformation. 
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3. GPRED Results 

Using the general parameter reduction process, possible complete parameter sets 
were determined from the 30 parameter, over-specified model. Before the calibration 
simulation and parameter reduction could be performed, the appropriate error function 
was defined. The error function for the ball bar system is similar to the linear slide, 


repeated here and illustrated in Figure 22. 
e=|d-]| (41) 
l 
d=[x?+y? +27}? (42) 


where the value / is the fixed length of the bar, and d is the distance between the end 


effector and the measurement system coordinate frame. 


(x,Y52) 


Figure 22. Ball bar kinematics. 


From GPRED, the rank of the over-specified model was 24, confirming that the 
complete model has 24 parameters. The output from GPRED showed that 17 possible 


parameter sets exist, Table 8, to include the identifiable parameter set developed by Driels 
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[Ref. 6]. The x markers in Table 8 indicate an identifiable parameter, with only the 
measurement system and end effector transformations shown, since no change occurred 


within the manipulator kinematic parameter set. 


Table 8. Complete model parameter sets for fixed length ball bar. 


The resulting parameter sets reveal that no change occurred in the measurement 
system kinematics, only the three translation parameters are identifiable. The end effector 
transformation followed along the same line as the modified linear slide end effector, 
requiring any three parameters, as long as one of the three is a translation. The parameter 
set in Table 9 was arbitrarily chosen in order to confirm that the other parameter sets 


actually do converge to an accurate solution of the actual kinematic parameters. 
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The simulation programs were modified by removing the effect of the 
non-identifiable parameters listed in Table 9, resulting in a 24 parameter model. The 
results of this 24 parameter simulation, with no noise injected, converged to within 1.0E° 
of the actual parameters. This confirms that the parameters listed in Table 9, added to the 
18 manipulator kinematic parameters, define a complete model of the PUMA with a ball 


bar constrained measurement system. Consequently, the other parameter sets listed in 


= 
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Table 9. Verified parameter set for the fixed length ball bar. 
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V. DISCUSSION OF RESULTS 


The general parameter reduction routine, GPRED, is able to determine complete 
model kinematic parameter sets by examining the identification Jacobian produced by the 
calibration simulation of an over-specified model. This was demonstrated by applying the 
process to the Driels and Pathre 5R1P manipulator and the PUMA 560 manipulator with 
two different partial pose, constrained measurement systems - the modified linear slide, 
and the fixed length ball bar. The results indicate that by starting with the 30 parameter 
over-specified model used in this work, the parameter reduction process can determine the 
complete, closed-chain kinematic model parameter set for any six degree of freedom 
mechanical linkage manipulator and measurement system, full or partial pose. The 
following explanation outlines how to use this method to determine the complete model 
kinematic parameter set. 

The first step in the process is to appropriately define the error function to be used 
in the simulation code. This error function is based on the actual type of measurement 
taken and the parameters in the world coordinate frame that relate to that measurement. 
Using the ball bar as an example, once the world coordinate frame is fixed to the lower 
end of the ball bar, the x,y,z coordinates of the other end are used to calculate a radial 
distance to that point. This distance is then compared to the length of the bar to determine 
the error (refer to equation 41). The x,y,z parameter values are determined from the 


forward kinematic solution transformation matrix, for the given set of joint angles, to 
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calculate the radial distance. The error function in the subroutine PUMA_ARM in ID6 
can then be modified to represent the measurement system employed. 

If the measurement system constrains the motion of the end effector, then the error 
function, in the program JOINT, must be modified in a similar way. The program JOINT 
generates the random joint data used in the simulation. The error function is used to 
ensure the joint data generated actually places the end effector within the constraints. 

If the manipulator under study incorporates a prismatic joint, as opposed to all 
revolute joints, then the joint variable definitions need to be modified to reflect the change. 
This is a simple, two-line change in each forward kinematic solution calculation, for which 
there is one in the sieaeil POSE and one in the subroutine PUMA_ARM. The program 
ID6P30, used to simulate the SR1P, provides an adequate example. 

If other than a six degree of freedom manipulator is being used, the standard 
Denavit-Hartenburg and Modified Denavit-Hartenburg transformations can be used to 
generate the over-specified model. The maximum number of parameters should not 
exceed the value of N from equation (43), where n is the total number of joints. 

N=4n+6 (43) 
The forward kinematic solutions would need to be modified to represent this manipulator. 

Once appropriate changes have been made to the simulation code, the simulation is 
performed, followed by the general parameter reduction routine GPRED. Appendix A 
contains GPRED for a 30 parameter over-specified model and ten levels of reduction. 


This satisfies the parameter limits described in equation (37). 
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GPRED outputs a series of vectors of non-identifiable parameters. Each set, when 
removed from the over-specified model, represents a non-unique model parameter set. 
The vectors contain the index values corresponding to the parameter vector element 
assignment in the program ID6. GPRED writes all the permutations of the dependent 
non-identifiable parameter vectors to DEPP.DAT. 

Having completed this process, any of the resulting parameter sets can be 
implemented to conduct the actual manipulator calibration. The number and types of 
observations, or measurements, needed to achieve an accurate calibration are addressed 
elsewhere. 

Although the simulation code allows noise to be injected into the simulated system, 
noise was not required to observe the Jacobian rank behavior of the systems investigated 
in this work. The effect of noise would require many more measurements to be simulated 
to prevent a significant reduction to the final accuracy. Noise effects on robot calibration 
and simulation have been studied elsewhere in depth. The only advantage to injecting 
noise in the system would be to determine whether the simulation phase (in order to 
determine the Jacobian approximation) of the general complete model determination 
process could be avoided. Instead experimental data may be used directly as an input to 
program ID6, and the general parameter reduction, GPRED, resulting in complete model 
parameter sets without required simulation. The new, complete model could then be 


implemented and the manipulator calibrated using the same data. 
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VI. CONCLUSIONS 


The results of the research reported in this thesis provide the following 
conclusions: 

1. A general method to determine a complete closed-chain model of any 
manipulator and measurement system, full or partial pose, beginning with an 
over-specified model, exists and works successfully. 

2 Any given, complete model parameter set for a robot manipulator is not 
unique. 

3. The identification Jacobian rank behavior directly correlates to parameter 
dependence and identifiability. 

4. The order of dependent parameter removal from the model has no bearing 


on the outcome of the final model. 


5, Changes in the manipulator configuration only affect the manipulator 
kinematics. 
6. Changes in the measurement system configuration only affect the 


measurement system kinematics. 
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APPENDIX A. PROGRAM GPRED 


Oi he ei he eee ee 2 2 ee es 
program gpred 


This program reduces a 30 parameter model pseudo 

inverse jacobian (jTj) by each parameter and investigates the 
changes in the rank of the reducei matrix IOT determine 

the minimum set of independent parameters 


aaaa 


INTEGER NN, NDP 
parametec(NN = 30, NDP = 30) 


REAL*8 JTJ(NN,NN), JRL(NN-L,NN-L), JR2(NN-2,NN-2) 
REAL*8 JR3(NN-3,NN-3), JR4(NN—4,NN-4), JRS(NN-3,NN-5) 
REAL*8 JR6(NN-6,NN-6), JR7(NN-7,NN-7), JR&(NN-9,NN-8) 
REAL*8 JRO(NN-~9,NN-9), JR1O(NN-LO,NN-10) 


INTEGER DP(NDP), DPI(NDP),DPF(NDP) 


REAL*8 U(NN,NN), V{NN,NN), TOL, S(NN) 
REAL*8 UL(NN-1,NN=1), VI(NN-L,NN-1), SI(NN-1L) 


REAL*8 U2(NN-2,NN-2), V2(NN~2,NN-2), S2(NN-2) 
REAL*8 U3(NN-3,NN—-3), V3(NN-3,NN-3), S3(NN-3) 
REAL*8 U4(NN-4,NN=—4), V4(NN-4,NN—-4), S4(NN-4) 
REAL*8 US(NN-5,NN-5), VS(NN-5,NN-5), SS(NN-5S) 
REAL*8 U6(NN-6,NN-6), V6(NN-6,NN-6), S6(NN-6) 


REAL*8 U7(NN-7,NN-7), V7(NN-7,NN-7), S7(NN-7) 
REAL*8 US(NN-8,NN-8), V8(NN-8,NN-8), S8(NN~8) 
REAL*8 U9(NN-~9,NN-9), V9(NN-9,NN-9), S9(NN-9) 
REAL*8 U1LQ(NN-10,NN-10), VLO(NN-1O,NN-10), SLO(NN-10) 


INTEGER SIZE, SIZEL, SIZE2, RJAC, N, RJTJ 
INTEGER Rl, R2, R3, R4, RS, RG, R7, RS, RY, RLO 
INTEGER 21, 22, 23, 24, 25, 26, 27, 28, 29, 210 
INTEGER IP, LVL, COUNT, D 

INTEGER I,II,I1Il,JJ,JJJ,K,m 


C write jTj to a file in column order 


OPEN (10,NAME=’DEPP_ 1ls.DAT’, STATUS=’NEW’ ) 
OPEN (15,NAME=/JTJ.DAT’, STATUS#’OLD’ } 


cead(15,*) 
cead(15,*)n,crjtj 


read(15,*) 
cead(15,*) 


do k = 1,N 
do kk = 1,N 
cead(15,*) jtej(kk,k) 
enddo 
enddo 


CLOSE (15) 
IP = 00 
TOL = 1.£-3 
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ZL=NN-1 

Z2 = Z1-1 
23 = 22-1 
Z4 = 23-1 
Z5 = Z4-1 
Z6 = 25-1 
Z7 = 26-1 
28 = 27-1 
Z9 = 28-1 
Z10 = 29-1 
COUNT = 1 
D = N-RJTJ 
m= il 


CALL DLSVRR(NN, NN, JTJ, NN, IP, TOL, RJAC, S, U, NN, V, NN) 
IF (RJAC .LT. N .AND. RUTJ .EQ. RJAC) THEN 


DO I = 1,NN 

WRITE(6,*) ‘’1-’,I,count,m 

m= m+l 

CALL MARED(JTJ,I,N,JR1) 

CALL DLSVRR(Z1,21,JR1L,NN=-L,IP, TOL,R1, 
& $1,U1,NN-1,V1L,NN-1) 


IF (R1 .£Q. RJAC) THEN 


LVL = 1 

DP(LVL) = f 

IF (D .EQ. LVL) THEN 
CALL SORTDP(DP,D) 
COUNT = COUNT+L 
GoTo 100 

END IF 


po II = 1,z1 


WRITE(6,*) ‘’2-’,II,count,m 

m= mel - 

CALL MARED(JR1,I1I,Z1,JR2) 

CALL DLSVRR(Z2,22,JR2,NN-2,IP, TOL,R2, 
& $2,U2,NN-2,V2,NN~2) 


IF (R2 .£Q. RJAC) THEN 


LVL = 2 

DP(LVL) = If 

IF (D .EQ. LVL) THEN 
CALL SORTDP(DP,D) 
COUNT = COUNT+1 
GOTO 200 

END IF 


DO III = 1,22 
WRITE(6,*) ’3-’,III,count,m 


m= m+l 
CALL MARED(JR2,II1I,Z2,JR3) 
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CALL DLSVRR(7Z3,23,3R3,NN-3,I1P, TOL,R3, 
$3,U3,NN-3,V3,NN-3) 


IF (R3 .EQ. RJAC) THEN 


LVL = 3 

DP(LVL) = III 

IF (D .EQ. LVL) THEN 
CALL SORTDP(DP,D) 
COUNT = COUNT+1 
GOTO 300 

END IF 


po JJ = 1,23 


WRITE(6,*) '4-',JJ,count,m 

m= m+l 

CALL MARED(JR3,J3J,23,79R4) 

CALL DOLSVRR(24,24,JR4,NN-4,I1P, TOL,R4, 
$4,U4,NN—-4,V4,NN—4) 


IF (R4 .EQ. RJAC) THEN 


LVL = 4 

DP(LVL) = JJ 

IF (D .£Q. LVL) THEN 
CALL SORTDP(DP,D) 
COUNT = COUNT+L 
GOTO 400 

END IF 


DO JJJ = 1,24 


WRITE(6,*) 'S-’,JJJ,count,m 

m= m+i 

CALL MARED(JR4,JJJ,24,IR5) 

CALL DLSVRR(25,25,JR5,NN—5,IP, TOL,RS, 
$5,05,NN-5,V5,NN-5) 


IF (RS .EQ. RJAC) THEN 


LVL = 5 

DP(LVL) = JJJ 

IF (D .£Q. LVL) THEN 
CALL SORTDP(DP,D) 
COUNT = COUNT+1 
GOTO $00 

END IF 


pO JII = 1,25 

WRITE(6,*) '6-’,JII,count,m 

m= m+i 

CALL MARED(JRS,JII,25,JR6) 

CALL DLSVRR(26,26,J3R6,NN-6,I1P, TOL,R6, 
$6,U6,NN~6,V6,NN~6) 

IF (R6 .EQ. RJAC) THEN 


LVL = 6 
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DP(LVL) = JIT 

IF (D .EQ. LVL) THEN 
CALL SORTDP(DP,D) 
COUNT = COUNT+1 
GOTO 600 

END IF 


po JJI = 1,26 


WRITE(6,*) ‘'7-'’,JJI,count,m 

ms m+l 

CALL MARED(JR6,JJ31,26,3R7) 

CALL DLSVRR(2Z7,27,3R7,NN-7,IP, TOL,R7, 
$7,U7,NN-7,V7,NN-7) 


IF (R7 .EQ. RJAC) THEN 


LVL = 7 

DP(LVL) = JJI 

IF (D .£Q. LVL) THEN 
CALL SORTDP(DP,D) 
COUNT = COUNT+1 
GOTO 700 

END IF 


pO JIJ = 1,27 


WRITE(6,*) ‘'8-’,JIJ,count,m 

m= m+l 

CALL MARED(JR7,JIJ,2Z7,IR8) 

CALL DLSVRR(2Z8,28,JR8,NN-8,IP, TOL,R8, 
$8,U8,NN-8,V8,NN-8) 


IF (R8 .EQ. RJAC) THEN 


LVL = 8 

DP(LVL) = JIJ 

IF (D .£Q. LVL) THEN 
CALL SORTDP(DP,D) 
COUNT = COUNT+1 
GOTO 800 

END IF 


DO IrJg = 1,28 


WRITE(6,*) ‘9-',IIJ,count,m 

m= m+l 

CALL MARED(JR&,IIJ,28,IR9) 

CALL DLSVRR(Z9,29,3R9,NN~-9,IP, TOL,R9, 
$9,U9,NN-9,V9,NN-9) 


IF (R9 .EQ. RJAC) THEN 


LVL = 9 

DP(LVL) = IIJ 

IF (D .£Q. LVL) THEN 
CALL SORTDP(DP,D) 
COUNT = COUNT+1 
GOTO 900 

END IF 
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1000 
900 
800 
700 
600 
500 
400 
300 
200 
100 


DO IJJ = 1,29 


WRITE(6,*) ‘10-',IJJ,count,m 

m= m+l 

CALL MARED(JR9,IJJ,29,J3R10) 

CALL OLSVRR(2Z10,210,3R10,NN-10,IP, TOL,RLO, 
$10,U10,NN-10,V10,NN-LO) 


IF (RLO .EQ. RJAC) THEN 


LVL = 10 

DP(LVL) = IJgJ 

If (D .£Q. LVL) THEN 
CALL SORTDP(DP,D) 
COUNT = COUNT+1 
GOTO 1000 

END IF 


END IF 
ENDDO 
END IF 
ENDDO 
END IF 
ENDDO 
END IF 
ENDDO 
END IF 
ENDDO 
END IF 
ENDDO 
END IF 
ENDDO 
END IF 
ENDDO 
END IF 
ENDDO 
END IF 
ENDDO 


END IF 
WRITE(10,*) ‘# parameter vectors =’,m 


END 


Cc Pee ec SSeS S ERE EEE E SEE eee oe ee eee ee ee el 


SUBROUTINE SORTDP (DP,DIF) 


INTEGER DIF, K, [, J, Ss, § 
INTEGER DP(DIF), DPI(10), DPF(10) 
parameter(ss = 30) 

integer pl(ss), cpl{ss-1) 


do i= 1,ss 
pl(i) =i 
enddo 


s = Ss 
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po J = 1,DI? 
call vred (dp(j),pl,s,rpl,dpi(j)) 


if (dp(1) .ne. dpi(1)) then 
write(6,*) ‘subroutine sort misfunction’ 


CALL SVIGN (DIF,DPI,DPF) 
WRITE(1LO,*) (DPF(JK), JK=#l,DIF) 
WRITE(5,*) (DP(JK), JK=l1,DIF) 
write(6,*) (dpi(jk), jk=l,dif) 
WRITE(6,*) (DPF(JK), JXal,DIF) 
RETURN 

END 


Cc fee ee OI IIIS TICK i ot 


subroutine maced(JT,IN,S,JNEW) 


integer S, IN, JR, JC, KR, KC 
ceal*8 JT(S,S), JNEW(S-1,S-1) 


JR = 1 
pO KR = 1,S-1l 
IF (KR.EQ.IN) THEN 
JR = JR+1 
END IF 
gc wl 
po Kc = 1,S—1 
IF (KC.EQ.IN) THEN 
jc = JC+l1 
END IF 


JNEW(KR,KC) = JT(JIR,JC) 
JC = JC+l 


ENDDO 


JR = JR+el 
ENDDO 
c WRITE(6,*) JT(1,1),7NEW(1,1) 
RETURN 
END 


fod de IIIS IOIOIOIOIOIOI OR TOI TORR IIR 
gubroutine vred(in,pli,s,plo,pv) 
integer in, s, pv, je, ke 


integer pli(s), plo(s-1l) 


py = pli(in) 
Jge=wl 
pO KC = 1,S-1 
IF (KC.EQ.IN) THEN 
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JO = JC+#1 


END IF 
plo(KC) = pli(gc) 
JC = JC+#l 

ENDDO 

return 

end 
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APPENDIX B. PROGRAM POSE FOR LINEAR SLIDE 


PROGRAM blinse 


This program generates joint angles for the Puma manipulator 

acm. It presumes that the tool frame of the manipulator is 
constrained to move in the positive x direction only. The 

tool is constrained by a ball joint mounted to a sliding lineac scale. 

The values along the x direction are determined by a random number 
generator. 


AAANANANA 


INTEGER LDFJAC, M, N, obs, nobs 
PARAMETER (LDFJAC#=3, M=LDFJAC, N=6) 


real*8 £i0, th0, sid, px0, py0, pz0 
REAL*8 OT1, DT2, DT3, DT4, OTS 

REAL*8 DDL, DD2, DD3, OD4, DDS 

REAL*8 AAlL, AA2, AA3, AA4, AAS 

REAL*8 ALL, AL2, AL3, AL4, ALS 

REAL*8 BL1, BL2, BL3, BL4, BLS 

REAL*8 DfF6, FI6, THE, SI6, PX6, PY6, PZ6 


REAL*8 RN1,RN2,RN3,RN4,RNS, RNG 
REAL*98 RN7,RN8,RN9,RN1LO,RN11,RN12 
REAL*8 RN13,RN14,RNL5,RN16,RN17,RN18 ee 


INTEGER infer,ier,iopt,nsig,maxfn 


REAL*8 FJAC(LDFJAC,N), xjtj((n+l)*n/2), xjac(ldfjac,n) 
REAL*8 parm(4), £(ldfjac), work((S*n)+(2*m)+( (n+l) *n/2)) 
REAL*8 X(N) 

REAL*8 magnx,magnl 


EXTERNAL PUMA_ARM 


INTEGER I, J, K, nou 
REAL*8 TDES(3), T(4,4), SCALE, DANGLE, DLENTH, NUM 


COMMON /PDATA/ TDES, DANGLE, DLENTH, T 

COMMON /KIN/ £i0, th0, sid, px0, py0, pzd, 
DT1,DT2,0T3,DT4,DTS, 
AL1,AL2,AL3,AL4,AL5, 
AA1,AA2,AA3,AA4,AAS, 
DDL,DD2,DD3,DD4,DD5, 
BL1,BL2,8L3,8L4,BL5, 
DF6,TH6,SI6,PX6,PY6,?26 


ROO we mM 


C Initialize data variables 
obs=0 
C Open data files for input 


OPEN (10, NAME=’puma-pos.dat’, STATUS='NEW’ ) 
OPEN (9, NAME=’input.dat’, STATUS=’OLD’) 


C Read input kinematic data 


cead (9,*) 
cead (9,*) f10,tn0,si0,px0,py0,pz0 
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cead 
read 
read 
cead 
cead 
cead 
read 


dtl,ddl,aal,all,b 

Mio ade ago a ets 
dt3,dd3,aa3,al13,b13 
dt4,dd4,aa4,al4,b14 
dtS,dd5,aaS,al5,bl15 


G£6,th&,siS,px6,ey5,p25 


read 
read 


wowwmwwowowowownwo 
a ar 
+ * + Re eH He HE 
ee ee ee ee 


NN 


close (9) 


C Adjust nominal values 


fid=wfid+dangle 
thowethO+dangle 
sid=sid+dangle 
px0=px0+dlenth 
pyO=py0+dlenth 
pzO#pz0+dlenth 


dtl=0.0 

dt2=#dt2+dangle 
dt3edt3+dangle 
dt4udtd+dangle 
dt5SadtS+dangle 


all=wali+dangle 
al2=al2+dangle 
al3#al3+dangle 
aldmal4+dangle 
alSmalS+dangle 


aalweaal+dlenth 
aa2waa2+dlenth 
aaj=maa3+dlenth 
aa4eaad+dlenth 
aaSmaaS+dlenth 


ddi=#0.0 
dd2=0.0 
dd3=add3+dlenth 
dd4mdd4+dlenth 
ddS=dd5+dlenth 


bllebll 
bl2=bl2+dangle 
bl3=bl13 
bl4=2b14 
blS=b15 


df6=dfé+dangle 
thé=thé+dangle 
si6=sif+dangle 
px6=px6+dlenth 
py6=pyS+dlenth 
pz6=pz6+dlenth 


C Get random number seed 


nobs,nou,dangle,dlenth,magnx,magnl 
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write (6 
read (5, 


iseed 
C Start of main loop 
1010 obs=*obs+l 


C Set joint angles to zero 


C Get random bar lengths 


1000 call random (iseed,num) 
num=num*940.0 


C Establish desired tool point 


TDES(1)= num 
TDES(2)= 0.0 
TDES(3)= 0.90 


C Call IMSL Z2XSSQ for inverse kinematic solution 


nsig=4 - 
eps=#0.0 
delta=0.0 
maxfn=500 
topt«l 
ixjaceldfjac 


a ‘Type in a 6-digit random number seed’ 


CALL ZXSSQ(puma_arm,m,n,nsig,eps,delta,maxfn,iopt,parm,x, 
& ssq,f,xjac,ixjac,xjtj,worck,infer,ier) 


C Check for singularities 
if (ssq .gt. 0.00001) goto 1000 
C Print results to 2 decimal places 
write(6,*) obs,ssq 
C Generate the random noise 


CALL RANDOM (ISEED,RN1) 
CALL RANDOM (ISEED,RN2) 
CALL RANDOM (ISEED,RN3) 
CALL RANDOM (ISEED,RN4) 
CALL RANDOM (ISEED,RNS) 
CALL RANDOM (ISEED,RN6) 
CALL RANDOM (ISEED,RN7) 
CALL RANDOM (ISEED,RN8) 
CALL RANDOM (ISEED,RN9) 
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aANA 


RN1 = MAGNX * (2.0 * RNI - 1.0) 
RN2 = MAGNX * (2.0 * RN2 - 1.0) 
RN3 = MAGNX * (2.0 * RN3 - 1.0) 
RN4 = MAGNI * (2.0 * RN4 - 1.0) 
RNS = MAGNL * (2.0 * RNS - 1.0) 
RN6 = MAGNIL * (2.0 * RN6 ~ 1.0) 
RN7 = MAGNIL * (2.0 * RN7T - 1.0) 
RN8 = MAGNIL * (2.0 * RNS - 1.0) 
RN9 = MAGNI * (2.0 * RNJ - 1.0) 
X(1) = X(1) + RN4 
X(2) = ¥(2) + RNS 
X(3) = X(3) + RNG 
X(4) = X(4) + RN7 
X(5) = X(5) + RNS 
X(6) = X(6) + RNO 
tdes(1l)=tdes(1l)+rnl 
tdes(2)=tdes(2)+rn2 
tdes(3)=etdes(3)+rcn3 
write (10,*) X(1),X(2),X%(3),X%(4),X(5),%(6) 
wreoite (1L0,*) tdes(l),tdes(2),tdes(3) 
write (10,*) 

Continue for other bar angles 
if (obs .lt. nobs) go to 1010 
CLOSE (10) 
END 
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SUBROUTINE PUMA_ARM (X,M,N,F) 


This subroutine calculates the non-lineac function for the use of 
the IMSL routine ZXSSQ. It is the forward kinematic solution for 
the PUMA manipulator. 


INTEGER M, N 


REAL*8 X(N), F(M) 

INTEGER II, JJ 

real*8 fid, thO, sid, px0, py0, pz0 

REAL*8 DT1, DT2, DT3, DT4, DTS 

REAL*8 DO1, DD2, DD3, DD4, DDS 

REAL*8 AAL, AA2, AA3, AA4, AAS 

REAL*8 AL1, AL2, AL3, AL4, ALS 

REAL*8 SLL, BL2, BL3, BL4, BLS 

REAL*8 DF6, FI6, TH6, SI6, PX6, PY6, P26 
REAL*8 TH1, TH2, TH3, TH4, THS 

REAL*8 T0(4,4), T1(4,4), 72(4,4), T3(4,4), T4(4,4) 
REAL*8 T5(4,4), T6(4,4), trpy(4,4), txyz(4,4) 
REAL*8 TIMAT(4,4), T(4,4), td(4,4) 
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INTEGER I, J, K 
REAL*8 TDES(3}, DANGLE, DLENTH, scale 


COMMON /PDATA/ TDES, DANGLE, DLENTH, T 

COMMON /KIN/ £10,th0,si0,5x0,py0,p20, 

i DT1L,OT2,0T3,0T4,DTS, 
ALL,AL2,AL3,AL4,ALS5, 
AAL,AA2,AA3,AA4,AAS, 
DD1,DD2,0D3,DD4,005, 
BL1,BL2,8L3,BL4,B825, 
DF6,TH6,SI6,?X6,PY6,P26 


—R HO MM Ww 


C Initialize the TIMAT matrix to an I matrix: 


DATA TIMAT/1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1/ 
scale=100.0 
C Initialize the T matrix to an f matrix 

po Ir = 1,4 

po JJ = 1,4 

T(II,dJ) = TIMAT(II,JJ) 

ENDDO 

ENDDO 


C Manipulator joint angles 


THL = DTL + K(1) 
TH2 = DT2 + X(2) 
TH3 = DT3 + X(3) 
TH4 = DT4 + X(4) 
THS = DTS + X(5) 
FI6 = DF6 + X(6) 


C Compute the T matrices, Tl thru T6: 


call t3rpy (£10,th0,si0, trpy) 
call t3xyz (px0,py0,pz0, txyz) 
call matmule (t0,trpy,txyz) 


ALL, AAl, DD1, TH1, BLL, Tl 
AL2, AA2, DD2, TH2, BL2, T2 
AL3, AA3, DD3, TH3, BL3, T3 
AL4, AA4, DD4, TH4, BL4, T4 
ALS, AAS, DDS, THS, BLS, TS 


CALL TRANSFORM 
CALL TRANSFORM 
CALL TRANSFORM 
CALL TRANSFORM 
CALL TRANSFORM 


CALL t3rpy ( £16, thé, si6, trpy ) 
CALL T3XYZ ( PX6, PY6, P26, txyz ) 
CALL matmulc ( t6, trpy, txyz ) 


C Compute the overall transformation, T: 


CALL MATMULA ( T, TO ) 
CALL MATMULA ( T, Tl ) 
CALL MATMULA ( T, T2 ) 
CALL MATMULA ( T, 73 ) 
CALL MATMULA ( T, T4 ) 
CALL MATMULA ( T, TS ) 
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an 


CALL MATMULA ( T, TS ) 


Calculate the function F 


do i=1,3 
xssq=zsum+f£(i) 
enddo 

weite (6,*) xssq 


RETURN 
END 
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SUBROUTINE RANDOM (x,2z) 


This subroutine generates random numbers in the range 0-1 
using a supplied seed x, the returned random number being z. 


REAL FM, FX, @ 
INTEGER A, X, I, M™ 
DATA I/1/ 


IF ( I .£Q. 9 ) GO TO 1000 
I=0 

Ma 2 ** 20 

FM= M 

Aw 2**10 + 3 


1000 X= MOD( A*X ,M) 
FX= X 
Z= FX/ FM 


RETURN 
END 
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APPENDIX C. PROGRAM ID6 FOR LINEAR SLIDE 


PROGRAM ID6 


Robot Identification using the Non-linear Least Squares method. 


Simulation data is read for the PUMA manipulator from. 
the data file PUMA-POS.DaT 


aAaANn 


C Change parameter LDFJAC to change the number of observations, 
C set LOFJAC = 6 * Number of observations 


INTEGER LOFJAC, MM, M, NN, N, NSIG, MAXFN, IOPT, ILXJAC, INFER, IER 
PARAMETER (LDFJAC=3*42, MM=@LDFJAC, NN=30) ; 


REAL*8 FJAC(LDFJAC,NN), XJTJ((NN+1)*NN/2) 

REAL*8 PARM(4), F(LDFJAC), WORK((S*NN)+(2*MM}+((NN+L) *NN/2}} 
REAL*8 X(NN) 

EXTERNAL PUMA_ARM 


REAL*8 DANGLE, DLENTH, TQ, DQ, EPS, DELTA, SSQ 
REAL*8 SQERR1, SQERR2Z 


real*8 £10,th0,si0,px0,py0,pz0 
REAL*8 DTL, DT2, DT3, DT4, DTS 
REAL*8 DO1l, DD2, DD3, DD4, DDS. 
REAL*8 AAL, AA2, AA3, AA4, AAS 
REAL*8 AL1, AL2, AL3, AL4, ALS 
REAL*8 BLL, BL2, BL3, BL4, BLS = ~ 
REAL*8 DF6, TH6, SI6, PX6, PYG, PZ6, FI6 


REAL*8 U(NN,NN), V(NN,NN), TOL, S(NN), FITJ(NN,NN) 
INTEGER NB, RJTJ, IPATH 


INTEGER I, J, K, NOBS, MAXNOBS 
PARAMETER (MAXNOBS#=360) 
REAL*8 TETL(MAXNOBS), TET2(MAXNOBS), TET3({MAXNOBS) 
REAL*8 TET4(MAXNOBS), TETS(MAXNOBS), TET6{MAXNOBS) 
REAL*8 TM(3,MAXNOBS), SCALE 
COMMON /PDATA/ NOBS, TM, SCALE, 

& TET1L, TET2, TET3, TET4, TETS, TET6 


C Open data files for inputs and results 


OPEN (8, NAME='RESULT.DAT’, STATUS='NEW’ } 
OPEN (9, NAME=’PUMA-POS.DAT', STATUS=’OLD’ ) 
OPEN (10,NAME=’INPUT.DAT’, STATUS#=’OLD’ ) 


c Read input parameters 


read (10,*) 

read (10,*) £10,th0,si0,px0,py0,pz0 
cead (10,*) dtl,ddl,aal,all,bll 
read (10,*) dt2,dd2,aa2,a12,b1l2 
read, (10,*) dt3,dd3,aa3,al13,b13 


cead (10,*) dt4,dd4,aa4,al4,bl4 

read (10,*) dt5,dd5,aa5,al15,b15 

read (10,*) 

read (10,*) d£6,th6,si6,px6,py6,pz6 

cead (10,*) 

read (10,*) nobs,n,dangle,dlenth,magnx,magnl 
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CLOSE (10) 
C write (6,*) ‘enter nobs’ 
c read (6,*} nobs 


C Initialize data variables 


X(17)<DT4 
X(18)<#0D4 
X(19)=AaA4 
*(20)=AL4 


C Read simulated joint data and tool pose 


DO J = 1, NOBS 


READ (9,*) TETL(J), TET2(J3), TET3(J), TET4(J), TETS(J), TET6(J) 
cead(9,*) TM(1L,J), TM(2,J), TM(3,J) : 
c read(9,*) 
c READ (9,*)a,b,c, TM(1,J) 
¢ read(9,*)a,b,c,TM(2,J) 
e cead(9,*)a,b,c,TM(3,J) 
c read(9,*) 
c read(9,*) 
Y ENDDO 
CLOSE (9) 


C Initialize scale for the angular rows of the Jacobian 
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SCALE#100.0 


C Call IMSL routine for non-linear identification 


NSIG=3 
EPS=0.0 
DELTA=0.0 
MAXFN=#1500 
TOPT#=1 
IXJAC#LDFJAC 
M=3*NOBS 


CALL 2XSSQ( PUMA_ARM,M,N,NSIG,EPS,DELTA,MAXFN,IOPT, 
& PARM,X, ssQ, F,FJAC, IXJAC, XJTJ,WORK,INFER, IER) 


€ Cale the jacobian transpose product and it’s cank; rank(jTj) 
NB = N 


IPATH = 00 
TOL = 1.E-6 


CALL DMXTXF(M, N, FJAC, LDFJAC, NB, FJTJ, NN) 
‘ Cc ' CALL MARANK(FJTJ, N, N, RJTJ) 
CALL DLSVRR(NN, NN, FUTJ, NN, IPATH, TOL, RJTJ, S, U, NN, V, NN) 


OPEN (15,NAME=’JTJ.DAT’, STATUS=’'NEW’ ) 
kk = 0 
k = 0 

C write jTj to a file in column order 


write(15,*)’mat size(square)’,’rank’ 
write(15,*)n,cjtj 
write(15,*) 
write(15,*)’jTj in column order’ 
do k = 1,N 

do kk = 1,N 

write(15,*) £jtj(kk,k) 

enddo 
enddo 
CLOSE (15) 


C Save results to data file 
WRITE (8,*) 


WRITE (8,*) ‘£10, thO, sid, px0, py0, pz0’ 
WRITE (8,889) X(1), X(2), X(3), X(4), x(5), x(6) 


WRITE (8,*) 

WRITE (8,*) ‘’DT1, DDL, AAL, ALl, BLL’ 

WRITE (8, rte 0.0, 0.0, X(7), X(8), 0.0 

WRITE (8,* 

WRITE lan ‘DT2, DD2, AA2, AL2, BL2’ 

WRITE (8,888) x(9), 0.0, X(10), X(11), X(12) 
WRITE (8,*) 

WRITE (8,*) ‘DT3, DD3, AA3, AL3, BL3’ 

WRITE (8,888) X(13), X(14), X(15), X(16), 0.0 
WRITE (8,*) 

WRITE (8,*) ‘DT4, DD4, AA4, AL4, BLA! 

WRITE (8,888) X(17), X(18), X(19), xX(20), 0.0 
WRITE (8,*) 

WRITE (8,*) ‘OTS, DDS, AAS, ALS, BLS’ 


WRITE (8,888) X(21), 


WRITE (8,*) 


X(22), X(23), X(24), 9.0 


WRITE (8,*) ‘'DF6, THE, SI6, PX6, PYS, P26’ 


‘WRITE (8,889) x(25), 


883 FORMAT ( 5F12.5 ) 
839 FORMAT ( 6F12.5 } 


x(26), x(27), x(28), x(29), *(30) 


C Calculate root mean square error in identification 


Cc 


Cc 


aaa 


TQ = DANGLE 
DQ = DLENTH 


Error in identification (angular parameters) 


SQERR1 = 

& (FIO+TQ-X(1))**2 +( THO+TQ—-X(2))**2 + 

& +(DT3+TQ-X(12))**2 +(DT4+TQ-X(16)) #*2 +(DT5+TQ—X(20))**2 
& +(AL1+TQ- -X(7))**2 +(AL2+TQ—X(10))**2 

& +(AL3+TQ-KX(15))**2 +(AL4+TQ-X(19))**2 +(AL5S+TQ-K(23))**2 
&  +(BL2+TQ-K(11L))**2 +(DT2+TQ-XK(8))**2 

& Herremier 

( 


SQERR1L = DSQRT 
Erroc in identification ( 


SQERR2 = 


(PX0+DQ-X(3))¥**2 +(AA1+0.0+DQ- 


-X 
(1 
+(PYO+DQ—X(4))**2 +(PZ0+DQ-X(5) 
(1 
x(2 


SQERRL/15 ) 


length parameters) 


)**2 +(AA2+DQ-K(9)) **2 
#2 +(AAS+DQ-K( 22) ) #*2 


(6) 
8)) 
)**2 
T))**2 +(DDS+DQ—-X(21))**2 
6))**2 


‘RMS PARMS (LENGTH), RMS PARMS (ANGLE), ’ 


SQERRI,’incorrect’ 


‘RMS PARMS (LENGTH), RMS PARMS (ANGLE)’ 
SQERR2,SQERRI,’incorrect’ 


IER,NOBS,NSIG, RANK’ 
IER,NOBS,NSIG,RJTJ 
IER,NOBS,NSIG, RANK’ 


& 
& +(AA3+DQ—-X(14))**2 +(AA4+DQ-X 
& 
& +(DD3+DQ-X(13))**2 +(DD4+DQ-X 
& +(px6+dq—-x(25))**2 +(pz6+dq-x 
SQERR2 = DSQRT( SQERR2Z/11 } 
WRITE (8,*) 
WRITE (8,*) 
WRITE (8,*) SQERR2, 
WRITE (6,*) 
WRITE (6,*) 
WRITE (8,*) 
WRITE (8,*) ‘INFER, 
WRITE (8,*) INFER, 
WRITE (6,*) ‘INFER, 
WRITE (6,*) INFER, 


WRITE (8,*) 
CLOSE (8) 


END 


4 


IER,NOBS,NSIG,RJTJ 
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SUBROUTINE PUMA_ARM (X, M, N, F) 


This subroutine calculates the non-linear function for the use of 


the IMSL routine DUNLSF. 
the PUMA manipulator. 


It is the forward kinematic solution for 
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INTEGER M, N 
REAL*8 X(N), F(M) 


INTEGER II, JJ 


real*8 f£10,th0,si0,px0,py0,pz0 
REAL*8 DTL, DT2, DT3, DT4, DTS 
REAL*8 ODL, DD2, DD3, DO4, DDS 
REAL*8 AAL, AA2, AA3, AAG, AAS 
REAL*8 ALL, AL2, AL3, AL4, ALS 
REAL*8 BL1, BL2, BL3, BL4, BLS 
REAL*8 FI6, TH6, SI6, PX6, PY6, PZ6, DFS 


REAL*8 TH1L, TH2, TH3, TH4, THS 
REAL*8 TO(4,4), T1(4,4), 72(4,4), 73(4,4), T4(4,4) 
REAL*8 T5(4,4), T6(4,4), TREY(4,4), TXY¥Z(4,4) 
REAL*8 TIMAT(4,4), T(4,4) 
REAL*8 TINV(4,4), TMJ(4,4), TDELTA(4,4) 

INTEGER [, J, K, NOBS, MAXNOBS 

PARAMETER (MAXNOBS=360) 

' REAL*8 TET1(MAXNOBS), TET2(MAXNOBS), TET3(MAXNOBS) 
REAL*8 TET4(MAXNOBS), TETS(MAXNOBS), TET6(MAXNOBS) 
REAL*8 TM(3,MAXNOBS), SCALE 
COMMON /PDATA/ NOBS, TM, SCALE, 

& TETL, TET2, TET3, TET4, TETS, TET6 


C Initialize the TIMAT matrix to an I matrix: 
DATA TIMAT/1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1/ 
C Set parameters for the manipulator: 


£i0 
thd 
sid 
px0 
pyo 
p20 


3 


3 
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AAd = X 
AL4 = X(20) 
BL4 = 0.0 
OTS = X(21) 
DDS = X(22) 
AAS = X(23) 
ALS = X(24) 
BLS = 0.0 
DF6 = x(25) 
THE = x(26) 
SI6 = x(27) 
px6 = (28) 
py6 = x(29) 
pz6 = x(30) 


C Loop NOBS times 


K = 0 
DO J = 1, NOBS 


i C Initialize the T matrix to an I matrix 


po rr = 1,3 
DO JJ = 1,4 
T(II,JJ) = TIMAT(ITI,JJ) 
ENDDO 
ENDDO 


C Manipulator joint angles 


THL = DTL + TET1(J) 
TH2 = DT2 + TET2(J) 
TH3 = DT3 + TET3(J) 
TH4 = DT4 + TET4(J) 
THS = DTS + TETS(J) 
FI6 = DFS + TET6(J) 


C Compute the T matrices, Tl thru T6: 


call t3rpy(fi0,th0,si0, trpy). 
call t3xyz(px0,py0,pz0, txyz) 
call matmule(t0,trpy,txyz) 


ALL, AAl, DDL, THI, BL1, Tl 
AL2, AA2, DD2, TH2Z, BL2, T2 
AL3, AA3, DD3, TH3, BL3, T3 
AL4, AA4, DD4, TH4, BL4, T4 
ALS, AAS, DDS, THS, BLS, TS 


CALL TRANSFORM 
CALL TRANSFORM 
CALL TRANSFORM 
CALL TRANSFORM 
CALL TRANSFORM 


oO nN 


CALL T3RPY ( FI6, TH6, SI6, TRPY ) 
CALL T3XY¥Z { PX6, PY6, PZ6, TXYZ } 
CALL MATMULC (T6, TRPY, TXYZ ) 


C Compute the overall transformation, T: 
CALL MATMULA ( T, TO ) 


CALL MATMULA ( T, Tl ) 
CALL MATMULA ( T, T2 ) 
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CALL MATMULA ( T, T3 ) 
CALL MATMULA ( T, T4 ) 
CALL MATMULA ( T, TS ) 
CALL MATMULA ( T, T6 ) 
E(kel)et(1,4)-tm(1,j) 
£(k+2)=t(2,4) 
E€(k+3)=t(3,4) 

kak+3 


C End the do-loop for counter J 
ENDDO 
C Write RMS error in F 
XSsQ=0.0 
DO IIawl,3*NOBS 
XSSQ=XSSQ+F(II) *F( IT) 
ENDOO 


XER#SQRT(XSSQ) 
WRITE(6,*) XER 


RETURN 
END 
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APPENDIX D. PROGRAM POSE FOR BALL BAR 


(a pdelielelieieieieitteieitet hte E LLL LEE TTC TT TT eeTeCTETLLTe 


aaanNn 


RD RM Mw 


C Joint 


PROGRAM PUMABAR 


This program generates a set of joint angles for tha calibration 
of the PUMA manipulator using a ball bar to constrain the end 
point of the manipulator. 


INTEGER LDFJAC, M, N, obs, nobs 
PARAMETER (LDFJAC#3, M=LDFJAC, N=6) 


REAL*8 DTL, DT2, DT3, DT4, DTS 

REAL*8 DD1, DD2, DD3, DD4, DDS 

REAL*8 AAl, AA2, AA3, AA4, AAS 

REAL*8 ALL, AL2, AL3, AL4, ALS 

REAL*8 BL1, BL2, BL3, BL4, BLS 

REAL*8 DFS, FI6, TH6, SI6, PX6, PY6, PZ6 
REAL*8 £10, thO, sid, px0, py0, pz0 


INTEGER infer,ier,iopt,nsig,maxfn 

REAL*8 FJAC(LDFJAC,N), xjtj((n+l)*n/2), xjac(ldéjac,n) 
REAL*8 parm(4), f(ldfjac), work((5*n)+(2*m)+((n+1)*n/2)) 
REAL*8 X(N) 

real*8 rc,phimax,phimin,thetamax,thetamin,phi,theta 
real*8 xb,yb,zb,ssq,cr,magnx,magnl 


EXTERNAL PUMA_ARM - 


INTEGER I, J, Kk 
REAL*8 TDES(4,4), qmax(6), qmin(6), SCALE, DANGLE, DLENTH, NUM 
COMMON /PDATA/ TDES, DANGLE, DLENTH, c 3 
COMMON /KIN/ DT1,DT2,DT3,DT4,DTS, 
AL1,AL2,AL3,AL4,AL5, 
AA1,AA2,AA3,AA4,AA5, 
DD1L,DD2,D0D3,D04,DD5, 
BL1,BL2,8L3,8L4,BL5, 
£i0,th0,si0,px0,py0,pz0, 
DF6,TH6,SI6,PX6,PY6, 226 


angle ranges 


data qmin/-160.0,-223.0,-52.0,-110.0,-100.0,-266.0/ 
data qmax/160.0, 43.0, 232.0, 170.0, 100.0, 266.0/ 


C Initialize data variables 


obs=0 


C Open data files for input 


C Read input kinematic data 


OPEN (10, NAME=’PUMA-SOLN.DAT’, STATUS='NEW’ ) 
open (9, NAME=’input.dat’, STATUS#‘old’) 


a 


cead (9,*) 

cead (9,*) £10,th0,si0,px0,py0,pz0 
cead (9,*) dtl,ddl,aal,all,bll 
cead (9,*) dt2,dd2,aa2,al2,b12 
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tread (9,*) dt3,dd3,aa3,al3,bl3 

read ($9,*) dt4,dd4,aa4,al4,b14 

read (9,*) dt5,ddS$,aa$,al5,bl5 

read (9,*) 

read (9,*) df6,th6,si6,px6, 

read (9,*) Pome ieee 

read (9,*) nobs,r,dangle,dlenth,magnx,magnl 
close (9) 


C Adjust nominal values 


fidsfid+dangle 
thdethd+dangle 
sid=msid+dangle 
px0=px0+dlenth 
py0=py0+dlenth 
pz0=pz0+dlenth 


dt2adt2+dangle 
dti=adt3+dangle 
dt4edt4+dangle 
dtS=adt5+dangle 


all=wall+dangle 
al2=al2+dangle 
al3mwal3+dangle 
al4deald+dangle 
alS=#al5+dangle 


aalewaal+dlenth 
aa2eaa2+dlenth 
aa3=maa3+dlenth 
aadeaad+dlenth 
aaSeaaS+dlenth 


dd3=dd3+dlenth 
dd4edd4+dlenth 
dd5=dd5+dlenth 


bl2=bl2+dangle 


df6é=df6+dangle 
thé=thé+dangle 
si6=sié+dangle 
px6=epx6+dlenth 
py6<py6+dlenth 
pzé6epz6+dlenth 


C Limits on bar rotation 
phimax=0.0 
phimin=-180.0 
thetamax=180.0 
chetamin=-180.0 

C Get candom number seed 


woite (6,*) ‘Type in a 6-digit random number seed’ 
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read (5,*) iseed 


C Write NOBS to PUMA-SOLN.DAT 


write (10,*) nobs 


C Start of main loop 


1010 


obs#obs+l 


C Set joint angles to zero 


do i=#l1,n 
x(i)=0.0 
enddo 


C Get random bar angles 


1000 


call candom (iseed,num) 
phi=phimin+(phimax~phimin)*num 

call random (iseed,num) 
thetaethetamin+({ thetamax—thetamin) *num 


C Calculate end point of the bar 


xberc*cosd( theta) *cosd(phi) 
yber*sind( theta) *cosd( phi) 
zbe-rc*sind(phi) 


C Reacheability calculation 


if (2 .1t. 0.0) go to 1000 


dx=xb-px0 

dy=yb-py0 

dz=zb-pz0 

creesqrt(dx*dx+dy*dy+dz*dz) 

if (re .gt. (AA2+0D4+PZ6)) go to 1000 


C Establish desired tool pose 


do iigl,4 

do. jj=l,4 
TDOES(ii,jj)=#0.0 

enddo 

enddo 


TDES(1,4)=xb 
TDES(2,4)=yb 
TDES(3,4)=zb 
TDES(4,4) = 1.0 


C Call IMSL'2XSSQ for inverse kinematic solution 


nsiged4 
eps=0.0 
delta=0.0 
maxfn=500 
iopt#1 
ixjaceldfjac 
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CALL. 2XSSQ(puma_arm,m,n,nsig,eps,delta,maxfn,iopt,pacm,x, 
: 8sq,f,xjac,ixjac,xjtj,work,inferc,ierc) 


C Check for singularities 
if (ssq .gt. 0.00001) goto 1900 


C Check joint angles are within ranges 


c do be) 6 
c weite(6,*) j,x(j) 

c pearls al )/360.0) 

ce x(j)=x(j i lreonaee. 3) 

C write(6,*)’cranged’,irev,x(j) 
C 

e enddo 


C Print results to 2 decimal places 

write(6,*) obs,ssq 

WRITE (10,888) X(1), X(2), X(3), K(4), X(5), (6) 
888 format ( 6£12.2 ) 


C Continue for other bar angles 


if (obs .1t. nobs) go to 1010 - 
CLOSE (10) 
END . 


C BRAKE RAH HAKKAR KR RAR eR RK ek 


SUBROUTINE PUMA_ARM (X,M,N,F)} 


C This subroutine calculates the non-linear function for the use of 
C the IMSL routine Z2XSSQ. It is the forward kinematic solution for 
C the PUMA manipulator. 


INTEGER M, N 
REAL*8 X(N), F(M) 


INTEGER II, JJ 

REAL*8 DT1, DT2, DT3, DT4, DTS 

REAL*8 DD1, DD2, DD3, DD4, DDS 

REAL*8 AAL, AA2, AA3, AA4, AAS 

REAL*8 ALL, AL2, AL3, AL4, ALS 

REAL*8 BLL, BL2, BL3, BL4, BLS 

REAL*8 DFS, FI6, TH6, SI6, PX6, PY&, PZ6 
REAL*8 £10, thO, sid, px0, py0, pz0 


REAL*8 TH1, TH2, TH3, TH4, THS 

REAL*8 T0(4,4), 71(4,4), T2(4,4), 73(4,4), T4(4,4) 
REAL*8 15(4,4), T6(4,4), trpy(4,4), txyz(4,4) 
REAL*8 TIMAT(4,4), T(4,4) 

REAL*8 disq,dis 


INTEGER I, J, K 
4 


REAL*8 TDES(4,4), DANGLE, DLENTH, c 
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COMMON /PDATA/ TDES, DANGLE, DLENTH, ct 

COMMON /KIN/ DT1,07T2,DT3,DT4,DT5, 
ALL,AL2,AL3,AL4,AL5, 
AAL,AA2,AA3,AA4,AA5, 
DD1,DD2,0D3,0D4,D05, 
BL1,BL2,8L3,8L4,8L5, 
£i0,th0,si0,px0,py0,pz0, 
DP6,TH6,SI6,PX6,PY5,PZ6 


RDO hM mw 


C Initialize the TIMAT matrix to an I matrix: 

DATA TIMAT/1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1/ 
C Initialize the T matrix to an I matrix 

DO [I = 1,4 

po JJ = 1,4 

T(II,JJ) = TIMAT(II,JJ) 
ENDDO 
ENDDO 


C Manipulator joint angles 


THL = DT1L + X(1) 
TH2 = DT2 + X(2) 
TH3 = DT3 + X(3) 
TH4 = DT4 + X(4) 
THS = DTS + X(5) 
FI6 = DF6 + X(6) 


C Compute the T matrices, Tl thru T6: 


call t3rcpy (£10,th0,si0, trpy) 
call t3xyz (px0,py0,pz20, txyz) 
call matmule (¢0,trpy,txyz) 


CALL TRANSFORM ( ALL, AA1, DD1, THI, BLL, Tl 
CALL TRANSFORM ( AL2, AA2, DD2, TH2, BL2, T2 
CALL TRANSFORM { AL3, AA3, DD3, TH3, BL3, T3 
CALL TRANSFORM ( AL4, AA4, DD4, TH4, BL4, T4 
CALL TRANSFORM ( ALS, AAS, DDS, THS, BLS, TS 


CALL t3cpy ( £16, thé, sié6, ttpy } 
CALL T3XY¥Z ( PX6, PY6, P26, txyz ) 
CALL matmule ( t6, trpy, txyz } 


C Compute the overall transformation, T: 


CALL MATMULA ( T, TO } 
CALL MATMULA ( T, Tl } 
CALL MATMULA ( T, T2 ) 
CALL MATMULA ( T, T3 ) 
CALL MATMULA ( T, T4 ) 
CALL MATMULA ( T, T5 ) 
CALL MATMULA ( T, T6 ) 


C. Calculate the function F 


E(L)=t(1,4)-tdes(1,4) 
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£(2)=t(2,4)-tdes(2,4) 
- £(3)=t(3,4)-tdes(3,4) 

RETURN 

END 


( pith hehehehe ee LEER EEE TREE EEL 


SUBROUTINE RANDOM (x,2z) 


c This subroutine generates candom numbers in the range 0-1 
C using a supplied seed x, the returned random number being 2. 


REAL FM, FX, 2 
INTEGER A, X, I, M 
DATA I/1/ 


Ir ( I .£Q. 0 ) GO TO 1000 
T=0 

M= 2 ** 20 

FM= M 

Am 2**10 + 3 


1000 K= MOD( A*X ,M) ee 
FX= X 
Z= FX/ FM 


RETURN 
END 
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APPENDIX E. PROGRAM ID6 FOR BALL BAR 


Cc FORGOT III IOI III ORI IOI IIIT OIC IOI III IOI ie 
PROGRAM ID6 

Robot Identification using the Non-linear Least Squares methed. 

Simulation data is read for the PUMA manipulator from 

the data file PUMA~soln.DaAT z 


Change parameter LDFJAC to change the number of observations, 
set LDFJAC = Number of obsecvations 


an aaAaA 


INTEGER LOFJAC, MM, M, NN, N, NSIG, MAXFN, IOPT, IXJAC, INFER, IER 
PARAMETER (LOFJAC#100, MM=LDFJAC, NN#=30) 


REAL*8 FJAC(LOFJAC,NN), XJTI((NN+1L)*NN/2) 

REAL*8 PARM(4), F(LOFJAC), WORK((S*NN)+(2*MM)+((NN+L)*NN/2) ) 
REAL*8 X(NN) . 

EXTERNAL PUMA_ARM 


REAL*8 DANGLE, DLENTH, TQ, 0Q, EPS, DELTA, SSQ 
REAL*8 SQERR1, SQERR2 

REAL*8 £10, th0, sid, px0, py0, pz0 

REAL*8 DT1, DT2, DT3, DT4, DTS 

REAL*8 DDL, DD2, DD3, DD4, DDS 

REAL*8 AAL, AA2, AA3, AA4, AAS 

REAL*8 ALL, AL2, AL3, AL4, ALS 

REAL*8 BL1, BL2, BL3, BL4, BLS 

REAL*8 FI6, DF6, TH6, SI6, PX6, PYS, PZ6 


REAL*8 U(NN,NN), V(NN,NN), TOL, S(NN), FUTJ(NN,NN) 
INTEGER NB, RJTJ, IPATH 


INTEGER I, J, K, NOBS, MAXNOBS 

REAL*8 magnx,magnl 

PARAMETER (MAXNOBS#100) 

REAL*8 TETL(MAXNOBS), TET2(MAXNOBS), TET3(MAXNOBS) 

REAL*8 TET4(MAXNOBS), TETS(MAXNOBS), TETS(MAXNOBS) 

REAL*8 R 

COMMON /PDATA/ NOBS, TET1, TET2, TET3, TET4, TETS, TETS, R 


COMMON /KIN/ DT1,DT2,DT3,DT4,DTS, 
AL1,AL2,AL3,AL4,AL5, 
AA1,AA2,AA3,AA4,AA5, 
DD1L,DD2,D03,0D4,0D5, 
BL1,BL2,8L3,8L4,8L5, 
£i10,th0,si0,px0,py0,pz0, 
DF6,TH6,SI6,PX6,PY6,PZ6 


ROM MM we 


C Open data files for inputs and results 
OPEN (8, NAME='’RESULT.DAT’, STATUS='NEW’ ) 


OPEN (9, NAME='PUMA-SOLN.DAT’, STATUS=’OQLD’ ) 
OPEN (10,NAME#{'INPUT.DAT’, STATUS#='OLD’) 


¢ Read input parameters 
read (10,*) 


read (10,*) £10,th0,si0,px0,py0,pz0 
read (10,*) dtl,ddl,aai,all,bll 


91 


read (10,*) dt2,dd2,aa2,al2,bl2 

read (10,*) dt3,dd3,aa3,al3,513 

read (10,*) dt4,dd4,aa4,al4,b14 

read (10,*) dtS,dd$,aa5,al5,b1$5 

read (10,*) 

read (10,*) df£6,th6,si6,px6,py$,p02$ 

cead (10,*) . ake 

read (10,*) nobs,r,dangle,dlenth,magnx,magnl 
CLOSE (10) 


C Initialize data variables 


3 


R=R+MAGNX 
C Read simulated joint data and tool pose 
READ (9,*) NOBS 
pO J = 1, NOBS 
READ (9,*) TETL(J), TET2(J), TET3(J), TET4(J), TETS(J), TET6(J) 


_ENDDO 
CLOSE (9) 
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C Call IMSL routine for non-linear identification 


NSIG=4 
EPS#0.0 
DELTA=0.0 
MAXFN#1000 
TOPT=1 
IXJAC=LDOFJAC 
M=NOB8S 


CALL Z2XSSQ({PUMA_ARM,M,NN,NSIG,EPS,DELTA,MAXFN, IOPT, 
& PARM,X,SSQ,F,FJAC,IXJAC,XJTJ,#ORK,INFER, IER) 


C Cale the jacobian transpose product and it’s cank; rank(jTj) 


NB = NN 
IPATH = 00 
TOL = 1.&-3 


CALL DMXTXF(M, NN, FJAC, LDFJAC, NB, FJTJ, NN) 
CALL DLSVRR(NN, NN, FITJ, NN, IPATH, TOL, RJTJ, S, U, NN, V, NN) 


OPEN (15,NAME=’JTJ.DAT’, STATUS#='NEW’ ) 
kk = 0 
k = 0 

C write jTj to a file in column order 


write(L5,*)’mat size(squarce)’,’rank’ 
write(15,*)nna,cjtj 
write(15,*) 
write(15,*)’jTj in column order’ 
do k = 1,NN 

do kk = 1,NN 

write(15,*) fjtj(kk,k) 

enddo 
enddo 
CLOSE (15) 


C Save results to data file 


WRITE (8,*) 

WRITE (8,*) ‘£10, thO, si0, px0, py0, pzo’ 
WRITE (8,*) KX(1), X(2), X(3), X(4), *(5), x(6) 
WRITE (8,*) 

WRITE (8,*) ‘DT1, DOL, AAL, ALL, BL1’ 

WRITE (8,*) 0.0, 0.0, X(7), X(8), 0.0 

WRITE (8,*) 

WRITE (8,*) 'DT2, DD2, AA2, AL2, BL2’ 

WRITE (8,*) X(9), 0.0, X(10), X(11), X(12) 
WRITE (8,*) 

WRITE (8,*) ‘'DT3, DD3, AA3, AL3, BL3’ 

WRITE (8,*) X(13), X(14), K(15), X(16), 0.0 
WRITE (8,*) 

WRITE (8,*) ‘OT4, DD4d, AA4, AL4, BL4’ 

WRITE (8,*) X(17), X(18), X(19), X(20), 0.0 
WRITE (8,*) 

WRITE (8,*) ‘OTS, DDS, AAS, ALS, BLS’ 

WRITE (8,*) X(21), X(22), %(23), K(24), 0.0 
WRITE (8,*) 
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WRITE (8,*) ‘DF6, THE, SI6, 2X6, PYS, PZ6’ 

WRITE (8,*) x(25), «(26), x(27), x(28), *(29), x*(30) 
48a FORMAT ( 5712.5 ) 
889 FORMAT ( 6F12.5 ) 


WRITE(8,*) ‘Re’,R 
c Restore initial values of input parameters 


open (10,name=’input.dat’,status=’old’) 


read (10,*) 

read (10,*) £10,th0,si0,px0,py0,p20 
read (10,*) dtl,ddl,aal,all,bll 
read (10,*) dt2,dd2,aa2,al2,bl2 
cead (10,*) at3,dd3,aa3,al3,b13 
read (10,*) dt4,dd4,aa4,al4,b14 
read (10,*) dt5,dd5,aa$,al5,bl5 
read (10,*) 

read (10,*) df£6,th6,si6,px6,py6,pz6 
cead (10,*) 

read (10,*) nobs,r,dangle,dlenth,magnx,magnl 
CLOSE (10) 


C Calculate root mean square error in identification 


TQ = DANGLE 
DQ = DLENTH 


C Error in identification (angular parameters) 


SQERR1 = 

& (DT2+TQ-X(6))**2 

& +(DT3+TQ-X(10))**2 +(DT4+TQ—K(14))**2 
& +(DT5+TQ-X(18))**2 

& re nee +(AL2+TQ=-X(8))**2 
& +(AL3+TQ-X(13))**2 +(AL4+TQ—X(17))**2 
& +(AL5+TQ-X(21))**2 +(BL2+TQ-X(9))**2 
& +(DF6+TQ-X(22))**2 


SQERR1 = DSQRT( SQERRI/11 ) | 


C Erroc in identification (length parameters) 


SQERR2 = 

& (AAL+DQ-X(4))** +(AA2+DQ—X(7))**2 

& +(AA3+DQ- EK toyed +(AA4+DQ—-X(16))**2 
& +(AA5+DQ—-X(20))**2 +(DD1+DQ—X(3))**2 

& +(DD3+DQ—-X(11))**2 +(DD4+DQ-K(15))**2 
& +(DD5+DQ-K(19))**2 +(PZ6+DQ—X(24))**2 
& +(PX6+DQ~K(23))**2 

& +(px0+dq—-x(1))**2 +(py0+dq-x(2))**2 


SQERR2Z = DSQRT( SQERR2/13 ) 


WRITE (8,*) 

WRITE (8,*) ‘RMS PARMS (LENGTH), RMS PARMS (ANGLE)’ 
WRITE (8,*) SQERR2, SQERRIL 

WRITE (6,*) ‘RMS PARMS (LENGTH), RMS PARMS (ANGLE)? 
WRITE (6,*) SQERR2,SQERR1 
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WRITE (8,*) 

WRITE (8,*) ‘INFER, IER,NOBS,NSIG,cank’ 
WRITE (8,*) INFER, IER,NOBS,NSIG,cjtj 
‘WRITE (6,*) ‘INFER, [TER,NOBS,NSIG, rank’ 
WRITE (6,*) INFER, IER,NOBS,NSIG,cjtj 
WRITE (8,*) 

CLOSE (8) 
END 


Ce ee FF FH Fe Fe THe Fe FIORE III IOI i 


SUBROUTINE PUMA_ARM (X, M, N, F) 


C This subroutine calculates the non-linear funetion for the use of 
C the IMSL routine ZxXSSQ. It is the forward kinematic solution for 
C the PUMA manipulator. 
INTEGER M, N 
r REAL*®8 X(N), F(M) 


INTEGER [I, JJ 


REAL*8 £10, th0, sid, px0, py0, p20 
REAL*8 DT1, DT2, DT3, DT4, OTS 

REAL*8 DDL, DD2, DD3, DD4, DDS 

REAL*8 AAL, AA2, AA3, AA4, AAS 

REAL*8 ALL, AL2, AL3, AL4, ALS 

REAL*8 BL1, BL2, BL3, BL4, BLS 

REAL*8 £16, df£6, thé, si6, PX6, PY6, PZ6 


REAL*8 TH1, TH2, TH3, TH4, THS 

REAL*8 T0(4,4), T1(4,4), T2(4,4), 73(4,4), T4(4,4) 
REAL*8 75(4,4), T6(4,4), trpy(4,4), txyz(4,4) 
REAL*8 TIMAT(4,4), 17(4,4) 


INTEGER I, J, K, NOBS, MAXNOBS 

PARAMETER (MAXNOBS=100) 

REAL*8 TET1(MAXNOBS), TET2(MAXNOBS), TET3(MAXNOBS) 

REAL*8 TET4(MAXNOBS), TETS(MAXNOBS), TET6(MAXNOBS) 

REAL*8 R, RR 

COMMON /PDATA/ NOBS, TET1, TET2, TET3, TET4, TETS, TETS, R 


COMMON /KIN/ DT1,DT2,D0T3,DT4,0TS, 
ALL,AL2,AL3,AL4,AL5, 
AAL,AA2,AA3,AA4,AA5, 
DD1L,DD2,0D3,DD4,DD5, 
BL1,8L2,8L3,84L4,BL5, 
fi0,th0,si0,px0,py0,pz0, 
DF6,TH6,SI6,PX6,PY6, P26 


RRR Mm wR 
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C Initialize the TIMAT matrix to an I matrix: 
DATA TIMAT/1,0,0,0,0,1,0,0,0,0,1,9,0,0,0,1/ 
C Set parameters for the manipulator: 
£id = X(1) 
tho = X(2) 


95 


ae] 
< 
Oo 
é@ua 


Easy BEE 


OP EE 
QU & Ww 
Sie ae 


tad 
oeuna 
OS OS MM MO DM 


Oo @~) 


RN 


5 
> 
oban @ 
OM KM 
WNYNNNNY ONNNN CONFER E 


wm 
*eean 
OM i 
aun 


a 

H 

n 
ae ako @ 
Owowoaan 


C Loop NOBS times 


K = 0 
DO J = 1, NOBS 


C Initialize the T matrix to an I matrix 


po Ir #= 1,4 
DO JJ = 1,4 
T(II,JJ) = TIMAT(II,JJ) 
ENDDO 
ENDDO 


Cc Manipulator joint angles 
THL = DTL + TETI(J) 


TH2 = DT2 + TET2(J) 
_TH3 = DT3 + TETI(J) 
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TH4 = DT4 + TET4(J) 
THS = DTS + TETS(J) 
FI6 = DF6 + TET6(j) 


C Compute the T matrices, Tl thru T6: 


call t3rpy(fid,th0,sid, trpy) 
call t3xyz(px0,py0,pz0, txyz) 
call matmulc(t0,tcpy,txyz) 


CALL TRANSFORM 
CALL TRANSFORM 
CALL TRANSFORM 
CALL TRANSFORM 
CALL TRANSFORM 


ALL, AAL, DDL, THL, BLL, Tl 
AL2, AA2, DD2, TH2, BL2, T2 
AL3, AA3, DD3, TH3, BL3, T3 
AL4, AA4, DD4d, TH4, BL4, T4 
ALS, AAS, DDS, THS, BLS, TS 


ee ee 


CALL t3rpy ( £16, thé, si6é, trpy ) 
CALL T3XY¥Z { PX6, PY6, PZ6, txyz ) 
CALL matmulc ( t6, trpy, txyz ) 


C Compute the overall transformation, T: 


CALL MATMULA ( T, TO ) 
CALL MATMULA ( T, Tl ) 
CALL MATMULA ( T, T2 } - 
CALL MATMULA ( T, T3 ) sa 
CALL MATMULA ( T, T4 ) 
CALL MATMULA ( T, TS ) 
CALL MATMULA ( T, T6 ) 


C Calculate the function F 


reredsqrt( b(1,4)*0(1,4)4b(2,4) *t(2,4)46(3,4)%t(3,4) ) 
f(j)=dabs( rr-r) 


C End the do-loop for counter J 
ENDDO 

C Compute RMS ecroc 
sumsq=0.0 
do j#l, nobs 
sumsq=sumsg+f(j)*f(j) 
enddo 
rms=sqrt(sumsq/nobs ) 
write (6,*) cms 


RETURN 
END 


4 
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